diff --git a/configs/sysid_defconfig b/configs/sysid_defconfig
new file mode 100644
index 0000000000..ce04a02fa8
--- /dev/null
+++ b/configs/sysid_defconfig
@@ -0,0 +1,6 @@
+CONFIG_PLATFORM_CF2=y
+CONFIG_DECK_RPM=y
+CONFIG_DECK_LOADCELL=y
+CONFIG_DECK_ACS37800=y
+CONFIG_ENABLE_THRUST_BAT_COMPENSATED=n
+CONFIG_DECK_FORCE="bcRpm:bcLoadcell:bcACS37800"
diff --git a/src/deck/drivers/src/Kbuild b/src/deck/drivers/src/Kbuild
index 52765dfab6..7594713d38 100644
--- a/src/deck/drivers/src/Kbuild
+++ b/src/deck/drivers/src/Kbuild
@@ -20,3 +20,7 @@ obj-$(CONFIG_DECK_USD) += usddeck.o
obj-$(CONFIG_DECK_ZRANGER) += zranger.o
obj-$(CONFIG_DECK_ZRANGER2) += zranger2.o
obj-$(CONFIG_DECK_CPX_HOST_ON_UART2) += cpx-host-on-uart2.o
+obj-$(CONFIG_DECK_RPM) += rpm.o
+obj-$(CONFIG_DECK_LOADCELL) += loadcell_nau7802.o
+obj-$(CONFIG_DECK_ACS37800) += acs37800.o
+
diff --git a/src/deck/drivers/src/Kconfig b/src/deck/drivers/src/Kconfig
index 1c4ee4ff58..08120fa350 100644
--- a/src/deck/drivers/src/Kconfig
+++ b/src/deck/drivers/src/Kconfig
@@ -476,3 +476,22 @@ config DECK_CPX_HOST_ON_UART2
forced, since this is intended to be used without actually having a proper
deck attached (i.e there will be no 1-wire memory to automatically detect
and start the driver).
+
+config DECK_ACS37800
+ bool "Support for the power measurement deck"
+ default n
+ help
+ Enables the support for the ACS37800 chip that can measure power/current/voltage.
+
+config DECK_RPM
+ bool "Support the rpm deck"
+ default n
+ help
+ Enables the rpm deck that can be usfull in testing.
+
+config DECK_LOADCELL
+ bool "Support for loadcell NAU7802"
+ default n
+ help
+ Enables the support for the NAU7802 loadcell.
+
\ No newline at end of file
diff --git a/src/deck/drivers/src/acs37800.c b/src/deck/drivers/src/acs37800.c
new file mode 100644
index 0000000000..d5f475e951
--- /dev/null
+++ b/src/deck/drivers/src/acs37800.c
@@ -0,0 +1,347 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * 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 .
+ *
+ * asc37800.c - Deck driver for ACS37800 power monitoring IC
+ */
+
+#include
+#include
+#include "stm32fxxx.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "timers.h"
+
+#include "debug.h"
+#include "system.h"
+#include "deck.h"
+#include "param.h"
+#include "log.h"
+#include "sleepus.h"
+#include "i2cdev.h"
+
+
+#define I_RANGE 30.0f // Sensor amp range
+#define V_DIVIDER 92 // (91k + 1k) / 1k
+#define ADC_RANGE 0.250f
+#define CODES_HALF_RANGE 27500.0f
+#define CODES_FULL_RANGE 55000.0f
+#define MILLW_TO_WATT 1000.0f
+#define LSB_PER_MILLWATT 3.08f
+
+// EEPROM
+#define ACSREG_E_TRIM 0x0B
+#define ACSREG_E_OFFS_AVG 0x0C
+#define ACSREG_E_FAULT 0x0D
+#define ACSREG_E_UNDR_OVR_V 0x0E
+#define ACSREG_E_IO_SEL 0x0F
+
+// SHADOW
+#define ACSREG_S_TRIM 0x1B
+#define ACSREG_S_OFFS_AVG 0x1C
+#define ACSREG_S_FAULT 0x1D
+#define ACSREG_S_UNDR_OVR_V 0x1E
+#define ACSREG_S_IO_SEL 0x1F
+
+#define ACSREG_I_V_RMS 0x20
+#define ACSREG_P_ACTIVE 0x21
+#define ACSREG_P_APPARENT 0x22
+#define ACSREG_NUM_PTS_OUT 0x25
+#define ACSREG_VRMS_AVGS 0x26
+#define ACSREG_VRMS_AVGM 0x27
+#define ACSREG_P_ACT_AVGS 0x28
+#define ACSREG_P_ACT_AVGM 0x29
+#define ACSREG_I_V_CODES 0x2A
+#define ACSREG_P_INSTANT 0x2C
+#define ACSREG_DSP_STATUS 0x2D
+#define ACSREG_ACCESS_CODE 0x2F
+
+#define ACS_I2C_ADDR 0x7F
+#define ACS_ACCESS_CODE 0x4F70656E
+
+
+static bool isInit;
+static uint32_t viBatRaw;
+static uint32_t viBatRMS;
+static uint32_t vavgBatRMS;
+static float vBat;
+static float vBatRMS;
+//static float vavgBat;
+static float iBat;
+static float iBatRMS;
+static float iavgBat;
+static uint32_t pBatRaw;
+static uint32_t pavgBatRaw;
+static float pBat;
+static float pavgBat;
+// "milli" representation to save logging bits
+static int16_t vBatMV;
+static int16_t iBatMA;
+static int16_t pBatMW;
+
+
+static uint16_t currZtrim, currZtrimOld;
+static uint8_t writeTrim;
+
+static void asc37800Task(void* prm);
+
+/*
+ * Sign extend a bitfield which if right justified
+ *
+ * data - the bitfield to be sign extended
+ * width - the width of the bitfield
+ * returns - the sign extended bitfield
+ */
+int32_t signExtendBitfield(uint32_t data, uint16_t width)
+{
+ // If the bitfield is the width of the variable, don't bother trying to sign extend (it already is)
+ if (width == 32)
+ {
+ return (int32_t)data;
+ }
+
+ int32_t x = (int32_t)data;
+ int32_t mask = 1L << (width - 1);
+
+ x = x & ((1 << width) - 1); // make sure the upper bits are zero
+
+ return (int32_t)((x ^ mask) - mask);
+}
+
+/*
+ * Convert an unsigned bitfield which is right justified, into a floating point number
+ *
+ * data - the bitfield to be converted
+ * binaryPoint - the binary point (the bit to the left of the binary point)
+ * width - the width of the bitfield
+ * returns - the floating point number
+ */
+float convertUnsignedFixedPoint(uint32_t inputValue, uint16_t binaryPoint, uint16_t width)
+{
+ uint32_t mask;
+
+ if (width == 32)
+ {
+ mask = 0xFFFFFFFF;
+ }
+ else
+ {
+ mask = (1UL << width) - 1UL;
+ }
+
+ return (float)(inputValue & mask) / (float)(1L << binaryPoint);
+}
+
+/*
+ * Convert a signed bitfield which is right justified, into a floating point number
+ *
+ * data - the bitfield to be sign extended then converted
+ * binaryPoint - the binary point (the bit to the left of the binary point)
+ * width - the width of the bitfield
+ * returns - the floating point number
+ */
+float convertSignedFixedPoint(uint32_t inputValue, uint16_t binaryPoint, uint16_t width)
+{
+ int32_t signedValue = signExtendBitfield(inputValue, width);
+ return (float)signedValue / (float)(1L << binaryPoint);
+}
+
+static bool asc37800Read32(uint8_t reg, uint32_t *data32)
+{
+ return i2cdevReadReg8(I2C1_DEV, ACS_I2C_ADDR, reg, 4, (uint8_t *)data32);
+}
+
+static bool asc37800Write32(uint8_t reg, uint32_t data32)
+{
+ return i2cdevWriteReg8(I2C1_DEV, ACS_I2C_ADDR, reg, 4, (uint8_t *)&data32);
+}
+
+static void ascFindAndSetAddress(void)
+{
+ uint8_t startAddr;
+ uint32_t dummy = 0;
+
+ for (startAddr = 96; startAddr <= 110; startAddr++)
+ {
+ bool isReplying = i2cdevWrite(I2C1_DEV, startAddr, 1, (uint8_t *)&dummy);
+
+ if (isReplying)
+ {
+ // Unlock EEPROM
+ dummy = ACS_ACCESS_CODE;
+ i2cdevWriteReg8(I2C1_DEV, startAddr, ACSREG_ACCESS_CODE, 4, (uint8_t *)&dummy);
+ // EEPROM: write and lock i2c address to 0x7F;
+ i2cdevReadReg8(I2C1_DEV, startAddr, ACSREG_E_IO_SEL, 4, (uint8_t *)&dummy);
+ DEBUG_PRINT("ACS37800 A:0x%.2X R:0x%.2X:%X\n", (unsigned int)startAddr, (unsigned int)ACSREG_E_IO_SEL, (unsigned int)dummy);
+
+ dummy = (0x7F << 2) | (0x01 << 9);
+ i2cdevWriteReg8(I2C1_DEV, startAddr, ACSREG_E_IO_SEL, 4, (uint8_t *)&dummy);
+ vTaskDelay(M2T(10));
+
+ DEBUG_PRINT("ACS37800 found on: %d. Setting address to 0x7E. Power cycle needed!\n", startAddr);
+ }
+ }
+}
+
+
+static void asc37800Init(DeckInfo *info)
+{
+ uint8_t dummy;
+ uint32_t val;
+
+ if (isInit) {
+ return;
+ }
+
+ if (i2cdevWrite(I2C1_DEV, ACS_I2C_ADDR, 1, (uint8_t *)&dummy))
+ {
+ asc37800Write32(ACSREG_ACCESS_CODE, ACS_ACCESS_CODE);
+ DEBUG_PRINT("ACS37800 I2C [OK]\n");
+ }
+ else
+ {
+ ascFindAndSetAddress();
+ }
+
+ // Enable bypass in shadow reg and set N to 32.
+ asc37800Write32(ACSREG_S_IO_SEL, (1 << 24) | (32 << 14) | (0x01 << 9) | (0x7F << 2));
+ // Set current and power for averaging and keep device specific (trim) settings.
+ asc37800Read32(ACSREG_S_TRIM, &val);
+ currZtrim = currZtrimOld = val & 0xFF;
+ asc37800Write32(ACSREG_S_TRIM, (val | (1 << 23) | (1 << 22)));
+ // Set average to 10 samples. (ASC37800 sample rate 1khz)
+ asc37800Write32(ACSREG_S_OFFS_AVG, ((10 << 7) | (10 << 0)));
+
+ DEBUG_PRINT("---------------\n");
+ for (int reg = 0x0B; reg <= 0x0F; reg++)
+ {
+ bool res;
+
+ res = asc37800Read32(reg, &val);
+ DEBUG_PRINT("%d:R:0x%.2X:%X\n", (unsigned int)res, (unsigned int)reg, (unsigned int)val);
+ res = asc37800Read32(reg+0x10, &val);
+ DEBUG_PRINT("%d:R:0x%.2X:%X\n\n", (unsigned int)res, (unsigned int)reg+0x10, (unsigned int)val);
+ }
+
+
+ xTaskCreate(asc37800Task, "asc37800",
+ 2*configMINIMAL_STACK_SIZE, NULL,
+ /*priority*/2, NULL);
+
+ isInit = true;
+}
+
+static void asc37800Task(void* prm)
+{
+ systemWaitStart();
+
+ TickType_t lastWakeTime = xTaskGetTickCount();
+
+ while(1) {
+ vTaskDelayUntil(&lastWakeTime, M2T(10));
+
+ asc37800Read32(ACSREG_I_V_CODES, &viBatRaw);
+ asc37800Read32(ACSREG_I_V_RMS, &viBatRMS);
+ asc37800Read32(ACSREG_VRMS_AVGS, &vavgBatRMS);
+ asc37800Read32(ACSREG_P_INSTANT, &pBatRaw);
+ asc37800Read32(ACSREG_P_ACT_AVGS, &pavgBatRaw);
+
+ vBat = (int16_t)(viBatRaw & 0xFFFF) / CODES_HALF_RANGE * ADC_RANGE * V_DIVIDER;
+ iBat = (int16_t)(viBatRaw >> 16 & 0xFFFF) / CODES_HALF_RANGE * I_RANGE;
+ vBatRMS = (uint16_t)(viBatRMS & 0xFFFF) / CODES_FULL_RANGE * ADC_RANGE * V_DIVIDER;
+ iBatRMS = (uint16_t)(viBatRMS >> 16 & 0xFFFF) / CODES_FULL_RANGE * I_RANGE;
+// vavgBat = (uint16_t)(vavgBatRMS & 0xFFFF) / CODES_FULL_RANGE * ADC_RANGE * V_DIVIDER;
+ iavgBat = (uint16_t)(vavgBatRMS >> 16 & 0xFFFF) / CODES_FULL_RANGE * I_RANGE;
+ pBat = (int16_t)(pBatRaw & 0xFFFF) / LSB_PER_MILLWATT * V_DIVIDER / MILLW_TO_WATT;
+ pavgBat = (int16_t)(pavgBatRaw & 0xFFFF) / LSB_PER_MILLWATT * V_DIVIDER / MILLW_TO_WATT;
+
+ vBatMV = (int16_t)(vBatRMS * 1000);
+ iBatMA = (int16_t)(iavgBat * 1000);
+ pBatMW = (int16_t)(pavgBat * 1000);
+
+ // Code so tuning can be done through cfclient parameters
+ if (currZtrimOld != currZtrim)
+ {
+ uint32_t val;
+ asc37800Read32(ACSREG_S_TRIM, &val);
+ DEBUG_PRINT("%X ->", (unsigned int)val);
+ val = (val & 0xFFFFFE00) | currZtrim;
+ DEBUG_PRINT("%X\n ", (unsigned int)val);
+ asc37800Write32(ACSREG_S_TRIM, val);
+ currZtrimOld = currZtrim;
+ }
+
+ if (writeTrim != 0)
+ {
+ uint32_t val;
+ writeTrim = 0;
+ // Enable EEPROM Writing
+ asc37800Write32(ACSREG_ACCESS_CODE, ACS_ACCESS_CODE);
+ asc37800Read32(ACSREG_S_TRIM, &val);
+ DEBUG_PRINT("EEPROM write: %X", (unsigned int)val);
+ // Write trim reg to EEPROM
+ asc37800Write32(ACSREG_E_TRIM, val);
+ vTaskDelay(M2T(10));
+ // Disable access
+ asc37800Write32(ACSREG_ACCESS_CODE, 0);
+ }
+
+// DEBUG_PRINT("V: %.3f I: %.3f P: %.3f\n", vBat, iBat, pBat);
+// DEBUG_PRINT("V: %f\tI: %f\tP: %f\n", vBat, iBat, pBat);
+// DEBUG_PRINT("Vc: %f\tIc: %f\tV: %f\tI: %f\tP: %f\n", vavgBat, iavgBat, vBat, iBat, pBat);
+ }
+}
+
+static const DeckDriver asc37800_deck = {
+ .vid = 0x00,
+ .pid = 0x00,
+ .name = "bcACS37800",
+
+ .usedPeriph = DECK_USING_I2C,
+
+ .init = asc37800Init,
+};
+
+DECK_DRIVER(asc37800_deck);
+
+PARAM_GROUP_START(deck)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcACS37800, &isInit)
+PARAM_GROUP_STOP(deck)
+
+PARAM_GROUP_START(asc37800)
+PARAM_ADD(PARAM_UINT16, currZtrim, &currZtrim)
+PARAM_ADD(PARAM_UINT8, writeTrim, &writeTrim)
+PARAM_GROUP_STOP(asc37800)
+
+LOG_GROUP_START(asc37800)
+LOG_ADD(LOG_FLOAT, v, &vBat)
+LOG_ADD(LOG_FLOAT, vRMS, &vBatRMS)
+LOG_ADD(LOG_INT16, v_mV, &vBatMV)
+LOG_ADD(LOG_FLOAT, i, &iBat)
+LOG_ADD(LOG_FLOAT, iRMS, &iBatRMS)
+LOG_ADD(LOG_FLOAT, i_avg, &iavgBat)
+LOG_ADD(LOG_INT16, i_mA, &iBatMA)
+LOG_ADD(LOG_FLOAT, p, &pBat)
+LOG_ADD(LOG_FLOAT, p_avg, &pavgBat)
+LOG_ADD(LOG_INT16, p_mW, &pBatMW)
+LOG_GROUP_STOP(asc37800)
diff --git a/src/deck/drivers/src/loadcell.c b/src/deck/drivers/src/loadcell.c
new file mode 100644
index 0000000000..dd0114128d
--- /dev/null
+++ b/src/deck/drivers/src/loadcell.c
@@ -0,0 +1,209 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * 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 .
+ *
+ * loadcell.c - Deck driver for HX711 load cell
+ * See
+ * * https://learn.sparkfun.com/tutorials/load-cell-amplifier-hx711-breakout-hookup-guide
+ * * Code based on https://github.com/bogde/HX711
+ */
+
+#include
+#include
+#include "stm32fxxx.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "timers.h"
+
+#include "deck.h"
+#include "param.h"
+#include "log.h"
+#include "sleepus.h"
+
+// Hardware defines (also update deck driver below!)
+#define CLK_PIN DECK_GPIO_IO1
+#define DAT_PIN DECK_GPIO_IO4
+
+static bool isInit;
+static int32_t rawWeight;
+static float weight;
+static bool enable = true;
+
+static float a = 4.22852802e-05;
+static float b = -2.21784688e+01;
+
+static void loadcellTask(void* prm);
+
+static enum hx711_gain
+{
+ GAIN128 = 1,
+ GAIN64 = 3,
+ GAIN32 = 2,
+} gain;
+
+static void hx711_init(void);
+static bool hx711_is_ready(void);
+// static void hx711_set_gain(void);
+static int32_t hx711_read(void);
+// static void hx711_power_down(void);
+static void hx711_power_up(void);
+
+static void hx711_init(void)
+{
+ pinMode(CLK_PIN, OUTPUT);
+ pinMode(DAT_PIN, INPUT);
+ // hx711_set_gain();
+ hx711_power_up();
+}
+
+static bool hx711_is_ready(void)
+{
+ return digitalRead(DAT_PIN) == LOW;
+}
+
+// static void hx711_set_gain(void)
+// {
+// digitalWrite(CLK_PIN, LOW);
+// hx711_read();
+// }
+
+static int32_t hx711_read(void)
+{
+ // Wait for the chip to become ready.
+ // while (!hx711_is_ready());
+
+ // if (!hx711_is_ready()) {
+ // return 0;
+ // }
+
+ // Define structures for reading data into.
+ int32_t value = 0;
+
+ // Protect the read sequence from system interrupts. If an interrupt occurs during
+ // the time the PD_SCK signal is high it will stretch the length of the clock pulse.
+ // If the total pulse time exceeds 60 uSec this will cause the HX711 to enter
+ // power down mode during the middle of the read sequence. While the device will
+ // wake up when PD_SCK goes low again, the reset starts a new conversion cycle which
+ // forces DOUT high until that cycle is completed.
+ //
+ // The result is that all subsequent bits read by shiftIn() will read back as 1,
+ // corrupting the value returned by read().
+
+ // Since we use a high-priority task, we do not need to actually disable the interrupt
+
+ // portDISABLE_INTERRUPTS();
+
+ // Pulse the clock pin 24 times to read the data.
+ for (int i = 0; i < 24; ++i) {
+ digitalWrite(CLK_PIN, HIGH);
+ value |= digitalRead(DAT_PIN) << (24 - i);
+ digitalWrite(CLK_PIN, LOW);
+ sleepus(1);
+ }
+
+ // Set the channel and the gain factor for the next reading using the clock pin.
+ for (int i = 0; i < gain; ++i) {
+ digitalWrite(CLK_PIN, HIGH);
+ sleepus(1);
+ digitalWrite(CLK_PIN, LOW);
+ sleepus(1);
+ }
+
+ // portENABLE_INTERRUPTS();
+
+ // Replicate the most significant bit to pad out a 32-bit signed integer
+ if (value & (1<<24)) {
+ value = 0xFF000000 | value;
+ }
+ return value;
+}
+
+// static void hx711_power_down(void)
+// {
+// digitalWrite(CLK_PIN, LOW);
+// digitalWrite(CLK_PIN, HIGH);
+// }
+
+static void hx711_power_up(void)
+{
+ digitalWrite(CLK_PIN, LOW);
+}
+
+static void loadcellInit(DeckInfo *info)
+{
+ if (isInit) {
+ return;
+ }
+
+ gain = GAIN128;
+ hx711_init();
+
+ // Create a task with very high priority to reduce risk that we get
+ // invalid data
+ xTaskCreate(loadcellTask, "LOADCELL",
+ configMINIMAL_STACK_SIZE, NULL,
+ /*priority*/6, NULL);
+
+ isInit = true;
+}
+
+static void loadcellTask(void* prm)
+{
+ TickType_t lastWakeTime = xTaskGetTickCount();
+
+ while(1) {
+ vTaskDelayUntil(&lastWakeTime, F2T(100));
+
+ if (enable && hx711_is_ready()) {
+ rawWeight = hx711_read();
+ weight = a * rawWeight + b;
+ }
+ }
+}
+
+static const DeckDriver loadcell_deck = {
+ .vid = 0x00,
+ .pid = 0x00,
+ .name = "bcLoadcell",
+
+ .usedGpio = DECK_USING_IO_1 | DECK_USING_IO_4,
+
+ .init = loadcellInit,
+};
+
+DECK_DRIVER(loadcell_deck);
+
+PARAM_GROUP_START(deck)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcLoadcell, &isInit)
+PARAM_GROUP_STOP(deck)
+
+PARAM_GROUP_START(loadcell)
+PARAM_ADD(PARAM_UINT8, enable, &enable)
+PARAM_ADD(PARAM_FLOAT, a, &a)
+PARAM_ADD(PARAM_FLOAT, b, &b)
+PARAM_GROUP_STOP(loadcell)
+
+LOG_GROUP_START(loadcell)
+LOG_ADD(LOG_INT32, rawWeight, &rawWeight)
+LOG_ADD(LOG_FLOAT, weight, &weight)
+LOG_GROUP_STOP(loadcell)
diff --git a/src/deck/drivers/src/loadcell_nau7802.c b/src/deck/drivers/src/loadcell_nau7802.c
new file mode 100644
index 0000000000..717aa780d5
--- /dev/null
+++ b/src/deck/drivers/src/loadcell_nau7802.c
@@ -0,0 +1,567 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * 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 .
+ *
+ * loadcell.c - Deck driver for NAU7802 load cell
+ * See
+ * * https://learn.sparkfun.com/tutorials/qwiic-scale-hookup-guide
+ * * Code based on https://github.com/sparkfun/SparkFun_Qwiic_Scale_NAU7802_Arduino_Library
+ */
+
+#include
+#include
+#include "nvicconf.h"
+#include "stm32fxxx.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "timers.h"
+
+#include "deck.h"
+#include "param.h"
+#include "log.h"
+#include "i2cdev.h"
+#include "sleepus.h"
+#include "debug.h"
+#include "statsCnt.h"
+
+// Hardware defines (also update deck driver below!)
+#define DECK_I2C_ADDRESS 0x2A
+#define DATA_READY_PIN DECK_GPIO_IO1
+
+static bool isInit;
+static int32_t rawWeight;
+static float weight;
+
+static float a[2];
+static float b[2];
+// static uint8_t currentChannel = 0;
+
+static uint8_t sampleRateDesired = 0;
+static uint8_t sampleRate = 0;
+static uint8_t channelDesired = 0;
+static uint8_t channel = 0;
+
+static STATS_CNT_RATE_DEFINE(rate, 1000);
+
+static xSemaphoreHandle dataReady;
+static StaticSemaphore_t dataReadyBuffer;
+
+static void loadcellTask(void* prm);
+
+////////////////////////////
+
+typedef struct
+{
+ uint8_t pu_ctrl;
+ uint8_t ctrl1;
+ uint8_t ctrl2;
+} nau7802_t;
+static nau7802_t nau7802;
+
+//Register Map
+typedef enum
+{
+ NAU7802_PU_CTRL = 0x00,
+ NAU7802_CTRL1,
+ NAU7802_CTRL2,
+ NAU7802_OCAL1_B2,
+ NAU7802_OCAL1_B1,
+ NAU7802_OCAL1_B0,
+ NAU7802_GCAL1_B3,
+ NAU7802_GCAL1_B2,
+ NAU7802_GCAL1_B1,
+ NAU7802_GCAL1_B0,
+ NAU7802_OCAL2_B2,
+ NAU7802_OCAL2_B1,
+ NAU7802_OCAL2_B0,
+ NAU7802_GCAL2_B3,
+ NAU7802_GCAL2_B2,
+ NAU7802_GCAL2_B1,
+ NAU7802_GCAL2_B0,
+ NAU7802_I2C_CONTROL,
+ NAU7802_ADCO_B2,
+ NAU7802_ADCO_B1,
+ NAU7802_ADCO_B0,
+ NAU7802_ADC = 0x15, //Shared ADC and OTP 32:24
+ NAU7802_OTP_B1, //OTP 23:16 or 7:0?
+ NAU7802_OTP_B0, //OTP 15:8
+ NAU7802_PGA = 0x1B,
+ NAU7802_PGA_PWR = 0x1C,
+ NAU7802_DEVICE_REV = 0x1F,
+} Scale_Registers;
+
+//Bits within the PU_CTRL register
+typedef enum
+{
+ NAU7802_PU_CTRL_RR = 0,
+ NAU7802_PU_CTRL_PUD,
+ NAU7802_PU_CTRL_PUA,
+ NAU7802_PU_CTRL_PUR,
+ NAU7802_PU_CTRL_CS,
+ NAU7802_PU_CTRL_CR,
+ NAU7802_PU_CTRL_OSCS,
+ NAU7802_PU_CTRL_AVDDS,
+} PU_CTRL_Bits;
+
+//Bits within the CTRL1 register
+typedef enum
+{
+ NAU7802_CTRL1_GAIN = 2,
+ NAU7802_CTRL1_VLDO = 5,
+ NAU7802_CTRL1_DRDY_SEL = 6,
+ NAU7802_CTRL1_CRP = 7,
+} CTRL1_Bits;
+
+//Bits within the CTRL2 register
+typedef enum
+{
+ NAU7802_CTRL2_CALMOD = 0,
+ NAU7802_CTRL2_CALS = 2,
+ NAU7802_CTRL2_CAL_ERROR = 3,
+ NAU7802_CTRL2_CRS = 4,
+ NAU7802_CTRL2_CHS = 7,
+} CTRL2_Bits;
+
+//Bits within the PGA register
+typedef enum
+{
+ NAU7802_PGA_CHP_DIS = 0,
+ NAU7802_PGA_INV = 3,
+ NAU7802_PGA_BYPASS_EN,
+ NAU7802_PGA_OUT_EN,
+ NAU7802_PGA_LDOMODE,
+ NAU7802_PGA_RD_OTP_SEL,
+} PGA_Bits;
+
+//Bits within the PGA PWR register
+typedef enum
+{
+ NAU7802_PGA_PWR_PGA_CURR = 0,
+ NAU7802_PGA_PWR_ADC_CURR = 2,
+ NAU7802_PGA_PWR_MSTR_BIAS_CURR = 4,
+ NAU7802_PGA_PWR_PGA_CAP_EN = 7,
+} PGA_PWR_Bits;
+
+//Allowed Low drop out regulator voltages
+typedef enum
+{
+ NAU7802_LDO_2V4 = 0b111,
+ NAU7802_LDO_2V7 = 0b110,
+ NAU7802_LDO_3V0 = 0b101,
+ NAU7802_LDO_3V3 = 0b100,
+ NAU7802_LDO_3V6 = 0b011,
+ NAU7802_LDO_3V9 = 0b010,
+ NAU7802_LDO_4V2 = 0b001,
+ NAU7802_LDO_4V5 = 0b000,
+} NAU7802_LDO_Values;
+
+//Allowed gains
+typedef enum
+{
+ NAU7802_GAIN_128 = 0b111,
+ NAU7802_GAIN_64 = 0b110,
+ NAU7802_GAIN_32 = 0b101,
+ NAU7802_GAIN_16 = 0b100,
+ NAU7802_GAIN_8 = 0b011,
+ NAU7802_GAIN_4 = 0b010,
+ NAU7802_GAIN_2 = 0b001,
+ NAU7802_GAIN_1 = 0b000,
+} NAU7802_Gain_Values;
+
+//Allowed samples per second
+typedef enum
+{
+ NAU7802_SPS_320 = 0b111,
+ NAU7802_SPS_80 = 0b011,
+ NAU7802_SPS_40 = 0b010,
+ NAU7802_SPS_20 = 0b001,
+ NAU7802_SPS_10 = 0b000,
+} NAU7802_SPS_Values;
+
+//Select between channel values
+typedef enum
+{
+ NAU7802_CHANNEL_1 = 0,
+ NAU7802_CHANNEL_2 = 1,
+} NAU7802_Channels;
+
+//Calibration state
+typedef enum
+{
+ NAU7802_CAL_SUCCESS = 0,
+ NAU7802_CAL_IN_PROGRESS = 1,
+ NAU7802_CAL_FAILURE = 2,
+} NAU7802_Cal_Status;
+
+void nau7802_init(nau7802_t* ctx)
+{
+ ctx->pu_ctrl = 0;
+ ctx->ctrl1 = 0;
+ ctx->ctrl2 = 0;
+}
+
+// //Resets all registers to Power Of Defaults
+// bool nau7802_reset(nau7802_t *ctx)
+// {
+// bool result = true;
+// result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_RR, 1);
+// sleepus(1);
+// result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_RR, 0);
+// return result;
+// }
+
+//Power up digital and analog sections of scale
+bool nau7802_powerUp(nau7802_t *ctx)
+{
+ bool result = true;
+
+ ctx->pu_ctrl = 0x06; //(PU analog and PU digital)
+ result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, ctx->pu_ctrl);
+ DEBUG_PRINT("pu %d\n", result);
+
+// result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUD, 1);
+// result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUA, 1);
+
+ //Wait for Power Up bit to be set - takes approximately 200us
+ for (int i = 0; i < 200; ++i) {
+ uint8_t bit;
+ result &= i2cdevReadBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_PUR, &bit);
+ if (result && bit) {
+ return true;
+ }
+ sleepus(1);
+ }
+ return false;
+}
+
+//Set the onboard Low-Drop-Out voltage regulator to a given value
+bool nau7802_setLDO(nau7802_t *ctx, NAU7802_LDO_Values ldoValue)
+{
+ bool result = true;
+ //Set the value of the LDO
+ ctx->ctrl1 &= 0b11000111; //Clear LDO bits
+ ctx->ctrl1 |= ldoValue << 3; //Mask in new LDO bits
+ result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL1, ctx->ctrl1);
+ //Enable the internal LDO
+ DEBUG_PRINT("l1 %d\n", ctx->pu_ctrl);
+ ctx->pu_ctrl |= (1 << NAU7802_PU_CTRL_AVDDS);
+ result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, ctx->pu_ctrl);
+ DEBUG_PRINT("l2 %d\n", ctx->pu_ctrl);
+ return result;
+}
+
+//Set the gain
+bool nau7802_setGain(nau7802_t *ctx, NAU7802_Gain_Values gainValue)
+{
+ bool result = true;
+ ctx->ctrl1 &= 0b11111000; //Clear gain bits
+ ctx->ctrl1 |= gainValue; //Mask in new bits
+ result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL1, ctx->ctrl1);
+ return result;
+}
+
+//Set the readings per second
+bool nau7802_setSampleRate(nau7802_t* ctx, NAU7802_SPS_Values rate)
+{
+ bool result = true;
+ ctx->ctrl2 &= 0b10001111; // Clear CRS bits
+ ctx->ctrl2 |= rate << 4; //Mask in new CRS bits
+ result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, ctx->ctrl2);
+ return result;
+}
+
+//Set channel
+bool nau7802_setChannel(nau7802_t *ctx, NAU7802_Channels channel)
+{
+ bool result = true;
+ ctx->ctrl2 &= 0b01111111; // Clear CHS bits
+ ctx->ctrl2 |= channel << 7; //Mask in new CHS bits
+ result &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, ctx->ctrl2);
+ return result;
+}
+
+//Check calibration status.
+NAU7802_Cal_Status nau7802_calAFEStatus(nau7802_t *ctx)
+{
+ bool result = true;
+ uint8_t bit;
+ result &= i2cdevReadBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, NAU7802_CTRL2_CALS, &bit);
+ if (result && bit) {
+ return NAU7802_CAL_IN_PROGRESS;
+ }
+ result &= i2cdevReadBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, NAU7802_CTRL2_CAL_ERROR, &bit);
+ if (result && bit) {
+ return NAU7802_CAL_FAILURE;
+ }
+
+ if (result) {
+ // Calibration passed
+ return NAU7802_CAL_SUCCESS;
+ }
+ return NAU7802_CAL_FAILURE;
+}
+
+//Calibrate analog front end of system. Returns true if CAL_ERR bit is 0 (no error)
+//Takes approximately 344ms to calibrate; wait up to 1000ms.
+//It is recommended that the AFE be re-calibrated any time the gain, SPS, or channel number is changed.
+bool nau7802_calibrateAFE(nau7802_t *ctx)
+{
+ bool result = true;
+ //Begin asynchronous calibration of the analog front end.
+ result &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, NAU7802_CTRL2_CALS, 1);
+
+ for (int i = 0; i < 1000; ++i) {
+ if (nau7802_calAFEStatus(ctx) == NAU7802_CAL_SUCCESS) {
+ return true;
+ }
+ vTaskDelay(M2T(1));
+ }
+ return false;
+}
+
+//Returns true if Cycle Ready bit is set (conversion is complete)
+bool nau7802_hasMeasurement(nau7802_t *ctx)
+{
+ bool result = true;
+ uint8_t bit;
+ result &= i2cdevReadBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, NAU7802_PU_CTRL_CR, &bit);
+ return result && bit;
+}
+
+//Returns 24-bit reading
+//Assumes CR Cycle Ready bit (ADC conversion complete) has been checked to be 1
+bool nau7802_getMeasurement(nau7802_t *ctx, int32_t *measurement)
+{
+ bool result = true;
+
+ uint8_t data[3];
+ result &= i2cdevReadReg8(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_ADCO_B2, 3, (uint8_t*)&data);
+ // DEBUG_PRINT("M: %d %d, %d, %d\n", result, data[0], data[1], data[2]);
+
+ int32_t valueRaw = ((int32_t)data[0] << 16) |
+ ((int32_t)data[1] << 8) |
+ ((int32_t)data[2] << 0);
+ // recover the sign
+ int32_t valueShifted = (valueRaw << 8);
+ int32_t value = (valueShifted >> 8);
+
+ *measurement = value;
+
+ return result;
+}
+
+bool nau7802_revision(nau7802_t *ctx, uint8_t *revision)
+{
+ bool result = true;
+ result &= i2cdevReadByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_DEVICE_REV, revision);
+ *revision = *revision & 0x0F;
+ return result;
+}
+
+/////////////////////////////
+
+static void setupDataReadyInterrupt(void)
+{
+ pinMode(DATA_READY_PIN, INPUT);
+
+ dataReady = xSemaphoreCreateBinaryStatic(&dataReadyBuffer);
+
+ // TODO: EXTI_PortSourceGPIOB and EXTI_PinSource8 should be part of deckGPIOMapping!
+ SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOB, EXTI_PinSource8);
+
+ EXTI_InitTypeDef EXTI_InitStructure;
+ EXTI_InitStructure.EXTI_Line = EXTI_Line8;
+ EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
+ EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
+ EXTI_InitStructure.EXTI_LineCmd = ENABLE;
+ // portDISABLE_INTERRUPTS();
+ EXTI_Init(&EXTI_InitStructure);
+ // EXTI_ClearITPendingBit(EXTI_Line15);
+ // portENABLE_INTERRUPTS();
+}
+
+void __attribute__((used)) EXTI8_Callback(void)
+{
+ portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+ xSemaphoreGiveFromISR(dataReady, &xHigherPriorityTaskWoken);
+ if (xHigherPriorityTaskWoken) {
+ portYIELD();
+ }
+}
+
+static void loadcellInit(DeckInfo *info)
+{
+ // if (isInit) {
+ // return;
+ // }
+
+ isInit = true;
+
+ // sleepus(1000 * 1000);
+ // isInit &= nau7802_reset();
+
+ if (true) {
+
+ nau7802_init(&nau7802);
+ // isInit &= nau7802_reset();
+ // DEBUG_PRINT("Reset [%d]\n", isInit);
+
+ isInit &= nau7802_powerUp(&nau7802);
+ DEBUG_PRINT("Power up [%d]\n", isInit);
+
+ uint8_t revision;
+ isInit &= nau7802_revision(&nau7802, & revision);
+ DEBUG_PRINT("revision [%d]: %d\n", isInit, revision);
+
+ isInit &= nau7802_setLDO(&nau7802, NAU7802_LDO_2V7);
+ DEBUG_PRINT("LDO [%d]\n", isInit);
+ isInit &= nau7802_setGain(&nau7802, NAU7802_GAIN_128);
+ DEBUG_PRINT("Gain [%d]\n", isInit);
+ isInit &= nau7802_setSampleRate(&nau7802, NAU7802_SPS_10);
+ DEBUG_PRINT("Sample rate [%d]\n", isInit);
+ // Turn off CLK_CHP. From 9.1 power on sequencing.
+ isInit &= i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_ADC, 0x30);
+ DEBUG_PRINT("CLK_CHP [%d]\n", isInit);
+ // Enable 330pF decoupling cap on chan 2. From 9.14 application circuit note.
+ isInit &= i2cdevWriteBit(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PGA_PWR, NAU7802_PGA_PWR_PGA_CAP_EN, 1);
+ DEBUG_PRINT("CAP [%d]\n", isInit);
+ // calibrate
+ isInit &= nau7802_calibrateAFE(&nau7802);
+ DEBUG_PRINT("Cal [%d]\n", isInit);
+
+ uint8_t v;
+ isInit &= i2cdevReadByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, &v);
+ DEBUG_PRINT("puctrl: %d\n", v);
+ isInit &= i2cdevReadByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL1, &v);
+ DEBUG_PRINT("ctrl1: %d\n", v);
+ isInit &= i2cdevReadByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_CTRL2, &v);
+ DEBUG_PRINT("ctrl2: %d\n", v);
+ }
+
+ // Set up Interrupt
+ setupDataReadyInterrupt();
+
+ if (1) {
+ // Create a task
+ xTaskCreate(loadcellTask, "LOADCELL",
+ configMINIMAL_STACK_SIZE, NULL,
+ /*priority*/1, NULL);
+ }
+}
+
+static void loadcellTask(void* prm)
+{
+ while (1) {
+ BaseType_t semResult = xSemaphoreTake(dataReady, M2T(500));
+ if (semResult == pdTRUE || digitalRead(DATA_READY_PIN))
+ {
+ int32_t measurement;
+ bool result = nau7802_getMeasurement(&nau7802, &measurement);
+ if (result) {
+ rawWeight = measurement;
+ weight = a[channel] * rawWeight + b[channel];
+ // currentChannel = (currentChannel + 1) % 2;
+ // nau7802_setChannel(&nau7802, currentChannel);
+
+ // nau7802.pu_ctrl |= (1 << NAU7802_PU_CTRL_CS);
+ // i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, nau7802.pu_ctrl);
+
+ // nau7802.ctrl2 &= 0b11101111; // Clear CS bit
+ // i2cdevWriteByte(I2C1_DEV, DECK_I2C_ADDRESS, NAU7802_PU_CTRL, nau7802.pu_ctrl);
+
+ STATS_CNT_RATE_EVENT(&rate);
+ }
+ }
+ if (sampleRate != sampleRateDesired) {
+ switch (sampleRateDesired) {
+ case NAU7802_SPS_320:
+ case NAU7802_SPS_80:
+ case NAU7802_SPS_40:
+ case NAU7802_SPS_20:
+ case NAU7802_SPS_10:
+ {
+ bool result = nau7802_setSampleRate(&nau7802, sampleRateDesired);
+ if (result) {
+ DEBUG_PRINT("switched sample rate!\n");
+ }
+ }
+ break;
+ default:
+ DEBUG_PRINT("Unknown sample rate!\n");
+ break;
+ }
+ sampleRate = sampleRateDesired;
+ }
+ if (channel != channelDesired) {
+ switch (channelDesired) {
+ case NAU7802_CHANNEL_1:
+ case NAU7802_CHANNEL_2:
+ {
+ bool result = nau7802_setChannel(&nau7802, channelDesired);
+ result &= nau7802_calibrateAFE(&nau7802);
+ if (result)
+ {
+ DEBUG_PRINT("switched and calibrated channel!\n");
+ }
+ }
+ break;
+ default:
+ DEBUG_PRINT("Unknown channel!\n");
+ break;
+ }
+ channel = channelDesired;
+ }
+
+
+ }
+}
+
+static const DeckDriver loadcell_deck = {
+ .vid = 0x00,
+ .pid = 0x00,
+ .name = "bcLoadcell",
+
+ .usedGpio = DECK_USING_IO_1,
+
+ .init = loadcellInit,
+};
+
+DECK_DRIVER(loadcell_deck);
+
+PARAM_GROUP_START(deck)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcLoadcell, &isInit)
+PARAM_GROUP_STOP(deck)
+
+PARAM_GROUP_START(loadcell)
+PARAM_ADD(PARAM_FLOAT, a, &a[0])
+PARAM_ADD(PARAM_FLOAT, b, &b[0])
+PARAM_ADD(PARAM_UINT8, sampleRate, &sampleRateDesired)
+PARAM_ADD(PARAM_UINT8, channel, &channelDesired)
+PARAM_GROUP_STOP(loadcell)
+
+LOG_GROUP_START(loadcell)
+LOG_ADD(LOG_INT32, rawWeight, &rawWeight)
+LOG_ADD(LOG_FLOAT, weight, &weight)
+
+STATS_CNT_RATE_LOG_ADD(rate, &rate)
+LOG_GROUP_STOP(loadcell)
diff --git a/src/deck/drivers/src/locodeck.c b/src/deck/drivers/src/locodeck.c
index a025c87c61..c7aab6021e 100644
--- a/src/deck/drivers/src/locodeck.c
+++ b/src/deck/drivers/src/locodeck.c
@@ -63,23 +63,23 @@
// LOCO deck alternative IRQ and RESET pins(IO_2, IO_3) instead of default (RX1, TX1), leaving UART1 free for use
#ifdef CONFIG_DECK_LOCODECK_USE_ALT_PINS
- #define GPIO_PIN_IRQ DECK_GPIO_IO2
+ #define GPIO_PIN_IRQ DECK_GPIO_IO2
#ifndef CONFIG_LOCODECK_ALT_PIN_RESET
- #define GPIO_PIN_RESET DECK_GPIO_IO3
+ #define GPIO_PIN_RESET DECK_GPIO_IO3
#else
- #define GPIO_PIN_RESET DECK_GPIO_IO4
+ #define GPIO_PIN_RESET DECK_GPIO_IO4
#endif
#define EXTI_PortSource EXTI_PortSourceGPIOB
- #define EXTI_PinSource EXTI_PinSource5
- #define EXTI_LineN EXTI_Line5
+ #define EXTI_PinSource EXTI_PinSource5
+ #define EXTI_LineN EXTI_Line5
#else
- #define GPIO_PIN_IRQ DECK_GPIO_RX1
- #define GPIO_PIN_RESET DECK_GPIO_TX1
+ #define GPIO_PIN_IRQ DECK_GPIO_RX1
+ #define GPIO_PIN_RESET DECK_GPIO_TX1
#define EXTI_PortSource EXTI_PortSourceGPIOC
- #define EXTI_PinSource EXTI_PinSource11
- #define EXTI_LineN EXTI_Line11
+ #define EXTI_PinSource EXTI_PinSource11
+ #define EXTI_LineN EXTI_Line11
#endif
diff --git a/src/deck/drivers/src/rpm.c b/src/deck/drivers/src/rpm.c
new file mode 100644
index 0000000000..4c9eb19f44
--- /dev/null
+++ b/src/deck/drivers/src/rpm.c
@@ -0,0 +1,328 @@
+/*
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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 .
+ *
+ * rpm.c - Deck that measure the motor RPM using QRD1114 IR reflector-sensor.
+ */
+#define DEBUG_MODULE "RPM"
+
+#include
+#include
+
+#include "stm32fxxx.h"
+#include "config.h"
+#include "deck.h"
+#include "debug.h"
+#include "log.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_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 ER_NBR_PINS 4
+
+typedef struct _etGpio
+{
+ GPIO_TypeDef *port;
+ uint16_t pin;
+ char name[6];
+} EtGpio;
+
+EtGpio erGpio[ER_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"},
+};
+
+static bool isInit;
+static uint16_t lastcc1Val;
+static uint16_t lastcc2Val;
+static uint16_t lastcc3Val;
+static uint16_t lastcc4Val;
+static uint16_t m1Time[2];
+static uint16_t m2Time[2];
+static uint16_t m3Time[2];
+static uint16_t m4Time[2];
+static int m1cnt;
+static int m2cnt;
+static int m3cnt;
+static int m4cnt;
+static uint16_t m1rpm;
+static uint16_t m2rpm;
+static uint16_t m3rpm;
+static uint16_t m4rpm;
+
+
+static void rpmInit(DeckInfo *info)
+{
+ int i;
+ isInit = true;
+
+ GPIO_InitTypeDef GPIO_InitStructure;
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+ TIM_ICInitTypeDef TIM_ICInitStructure;
+ NVIC_InitTypeDef NVIC_InitStructure;
+
+ // Enable Clocks
+ RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB, ENABLE);
+ RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM5, ENABLE);
+
+ // Configure optical switch input pins
+ for (i = 0; i < ER_NBR_PINS; i++)
+ {
+ GPIO_InitStructure.GPIO_Pin = erGpio[i].pin;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+ GPIO_InitStructure.GPIO_Speed = GPIO_High_Speed;
+ GPIO_Init(erGpio[i].port, &GPIO_InitStructure);
+ }
+
+ // Map timer to alternate functions
+ GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_TIM5);
+ GPIO_PinAFConfig(GPIOA, GPIO_PinSource3, GPIO_AF_TIM5);
+ GPIO_PinAFConfig(GPIOB, GPIO_PinSource5, GPIO_AF_TIM3);
+ GPIO_PinAFConfig(GPIOB, GPIO_PinSource4, GPIO_AF_TIM3);
+
+ /* Time base configuration */
+ TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
+ TIM_TimeBaseStructure.TIM_Prescaler = (84 - 1); // 84 / 84M = 1uS
+ TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+ TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);
+ TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
+
+ TIM_ICInitStructure.TIM_Channel = TIM_Channel_3;
+ TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
+ TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
+ TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
+ TIM_ICInitStructure.TIM_ICFilter = 0x0;
+ TIM_ICInit(TIM5, &TIM_ICInitStructure);
+ TIM_ICInitStructure.TIM_Channel = TIM_Channel_4;
+ TIM_ICInit(TIM5, &TIM_ICInitStructure);
+ TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
+ TIM_ICInit(TIM3, &TIM_ICInitStructure);
+ TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
+ TIM_ICInit(TIM3, &TIM_ICInitStructure);
+
+ /* Enable the TIM5 and TIM3 global Interrupt */
+ NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 10;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+ NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
+ NVIC_Init(&NVIC_InitStructure);
+
+ /* TIM enable counter */
+ TIM_Cmd(TIM5, ENABLE);
+ TIM_Cmd(TIM3, ENABLE);
+ TIM_ITConfig(TIM5, TIM_IT_CC3, ENABLE);
+ TIM_ITConfig(TIM5, TIM_IT_CC4, ENABLE);
+ TIM_ITConfig(TIM5, TIM_IT_Update, ENABLE);
+ TIM_ITConfig(TIM3, TIM_IT_CC1, ENABLE);
+ TIM_ITConfig(TIM3, TIM_IT_CC2, ENABLE);
+ TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
+}
+
+static uint16_t calcRPM(uint32_t t1, uint32_t t2)
+{
+ uint16_t rpm;
+ // (timer resolution * sec/min) / (time for one revolution)
+ rpm = (1000000 * 60) / (t1 + t2);
+
+ return rpm;
+}
+
+void __attribute__((used)) TIM5_IRQHandler(void)
+{
+ static int updateM1Cnt = 2;
+ static int updateM4Cnt = 2;
+ uint16_t ccVal;
+
+ //Motor1
+ if(TIM_GetITStatus(TIM5, TIM_IT_CC3) == SET)
+ {
+ /* Clear TIM5 Capture compare interrupt pending bit */
+ TIM_ClearITPendingBit(TIM5, TIM_IT_CC3);
+ ccVal = TIM_GetCapture3(TIM5);
+
+ if (TIM_GetFlagStatus(TIM5, TIM_FLAG_CC3OF))
+ {
+ // Overflow
+ lastcc3Val = ccVal;
+ }
+
+ if (ccVal > lastcc3Val)
+ m1Time[m1cnt & 0x01] = ccVal - lastcc3Val;
+ else
+ m1Time[m1cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc3Val);
+
+ lastcc3Val = ccVal;
+ m1cnt++;
+ m1rpm = calcRPM(m1Time[0], m1Time[1]);
+ updateM1Cnt = 2;
+ }
+
+ //Motor4
+ if(TIM_GetITStatus(TIM5, TIM_IT_CC4) == SET)
+ {
+ /* Clear TIM5 Capture compare interrupt pending bit */
+ TIM_ClearITPendingBit(TIM5, TIM_IT_CC4);
+ ccVal = TIM_GetCapture4(TIM5);
+
+ if (TIM_GetFlagStatus(TIM5, TIM_FLAG_CC4OF))
+ {
+ // Overflow
+ lastcc4Val = ccVal;
+ }
+
+ if (ccVal > lastcc4Val)
+ m4Time[m4cnt & 0x01] = ccVal - lastcc4Val;
+ else
+ m4Time[m4cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc4Val);
+
+ lastcc4Val = ccVal;
+ m4cnt++;
+ m4rpm = calcRPM(m4Time[0], m4Time[1]);
+ updateM4Cnt = 2;
+ }
+
+ if(TIM_GetITStatus(TIM5, TIM_IT_Update) == SET)
+ {
+ TIM_ClearITPendingBit(TIM5, TIM_IT_Update);
+ if (--updateM1Cnt < 0)
+ {
+ m1rpm = 0;
+ }
+ if (--updateM4Cnt < 0)
+ {
+ m4rpm = 0;
+ }
+ }
+}
+
+void __attribute__((used)) TIM3_IRQHandler(void)
+{
+ static int updateM2Cnt = 2;
+ static int updateM3Cnt = 2;
+ uint16_t ccVal;
+
+ //Motor2
+ if(TIM_GetITStatus(TIM3, TIM_IT_CC1) == SET)
+ {
+ /* Clear TIM3 Capture compare interrupt pending bit */
+ TIM_ClearITPendingBit(TIM3, TIM_IT_CC1);
+ ccVal = TIM_GetCapture1(TIM3);
+
+ if (TIM_GetFlagStatus(TIM3, TIM_FLAG_CC1OF))
+ {
+ // Overflow
+ lastcc1Val = ccVal;
+ }
+
+ if (ccVal > lastcc1Val)
+ m2Time[m2cnt & 0x01] = ccVal - lastcc1Val;
+ else
+ m2Time[m2cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc1Val);
+
+ lastcc1Val = ccVal;
+ m2cnt++;
+ m2rpm = calcRPM(m2Time[0], m2Time[1]);
+ updateM2Cnt = 2;
+
+// rpmPutchar((m2Time >> 8) & 0x00FF);
+// rpmPutchar(m2Time & 0x00FF);
+ }
+
+ //Motor3
+ if(TIM_GetITStatus(TIM3, TIM_IT_CC2) == SET)
+ {
+ /* Clear TIM3 Capture compare interrupt pending bit */
+ TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
+ ccVal = TIM_GetCapture2(TIM3);
+
+ if (TIM_GetFlagStatus(TIM3, TIM_FLAG_CC2OF))
+ {
+ // Overflow
+ lastcc2Val = ccVal;
+ }
+
+ if (ccVal > lastcc2Val)
+ m3Time[m3cnt & 0x01] = ccVal - lastcc2Val;
+ else
+ m3Time[m3cnt & 0x01] = (uint16_t)((0xFFFF + (uint32_t)ccVal) - lastcc2Val);
+
+ lastcc2Val = ccVal;
+ m3cnt++;
+ m3rpm = calcRPM(m3Time[0], m3Time[1]);
+ updateM3Cnt = 2;
+
+// rpmPutchar((m3rpm >> 8) & 0x00FF);
+// rpmPutchar(m3rpm & 0x00FF);
+ }
+
+ if(TIM_GetITStatus(TIM3, TIM_IT_Update) == SET)
+ {
+ TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
+ if (--updateM2Cnt < 0)
+ {
+ m2rpm = 0;
+ }
+ if (--updateM3Cnt < 0)
+ {
+ m3rpm = 0;
+ }
+ }
+}
+
+static const DeckDriver rpm_deck = {
+ .vid = 0x00,
+ .pid = 0x00,
+ .name = "bcRpm",
+
+ .usedGpio = DECK_USING_IO_2 | DECK_USING_IO_3 | DECK_USING_PA2 | DECK_USING_PA3,
+
+ .init = rpmInit,
+};
+
+DECK_DRIVER(rpm_deck);
+
+
+LOG_GROUP_START(rpm)
+LOG_ADD(LOG_UINT16, m1, &m1rpm)
+LOG_ADD(LOG_UINT16, m2, &m2rpm)
+LOG_ADD(LOG_UINT16, m3, &m3rpm)
+LOG_ADD(LOG_UINT16, m4, &m4rpm)
+LOG_GROUP_STOP(rpm)
+
diff --git a/src/drivers/interface/motors.h b/src/drivers/interface/motors.h
index 0a97cfbd7d..e480123dd2 100644
--- a/src/drivers/interface/motors.h
+++ b/src/drivers/interface/motors.h
@@ -58,7 +58,6 @@
#define MOTORS_TIM_DBG_CFG DBGMCU_APB2PeriphConfig
#define MOTORS_GPIO_AF_CFG(a,b,c) GPIO_PinAFConfig(a,b,c)
-
#ifdef CONFIG_MOTORS_ESC_PROTOCOL_ONESHOT125
/**
* *WARNING* Make sure the brushless driver is configured correctly as on the Crazyflie with normal
diff --git a/src/drivers/src/i2c_drv.c b/src/drivers/src/i2c_drv.c
index 31c9807eb1..6eab7ec270 100644
--- a/src/drivers/src/i2c_drv.c
+++ b/src/drivers/src/i2c_drv.c
@@ -623,21 +623,20 @@ static void i2cdrvClearDMA(I2cDrv* i2c)
static void i2cdrvDmaIsrHandler(I2cDrv* i2c)
{
- if (DMA_GetFlagStatus(i2c->def->dmaRxStream, i2c->def->dmaRxTCFlag)) // Transfer complete
- {
- i2cdrvClearDMA(i2c);
- i2cNotifyClient(i2c);
- // Are there any other messages to transact?
- i2cTryNextMessage(i2c);
- }
if (DMA_GetFlagStatus(i2c->def->dmaRxStream, i2c->def->dmaRxTEFlag)) // Transfer error
{
DMA_ClearITPendingBit(i2c->def->dmaRxStream, i2c->def->dmaRxTEFlag);
//TODO: Best thing we could do?
i2c->txMessage.status = i2cNack;
- i2cNotifyClient(i2c);
- i2cTryNextMessage(i2c);
}
+ if (DMA_GetFlagStatus(i2c->def->dmaRxStream, i2c->def->dmaRxTCFlag)) // Transfer complete
+ {
+ i2c->txMessage.status = i2cAck;
+ }
+ i2cdrvClearDMA(i2c);
+ i2cNotifyClient(i2c);
+ // Are there any other messages to transact?
+ i2cTryNextMessage(i2c);
}
diff --git a/src/drivers/src/motors.c b/src/drivers/src/motors.c
index 14f2b844cb..dc2431820e 100644
--- a/src/drivers/src/motors.c
+++ b/src/drivers/src/motors.c
@@ -47,7 +47,7 @@
#include "log.h"
#include "param.h"
-static bool motorSetEnable = false;
+static uint8_t motorSetEnable = 0;
static uint16_t motorPowerSet[] = {0, 0, 0, 0}; // user-requested PWM signals (overrides)
static uint32_t motor_ratios[] = {0, 0, 0, 0}; // actual PWM signals
@@ -99,6 +99,7 @@ static bool isInit = false;
static uint64_t lastCycleTime;
static uint32_t cycleTime;
+
/* Private functions */
#ifndef CONFIG_MOTORS_ESC_PROTOCOL_DSHOT
@@ -242,6 +243,13 @@ void motorsInit(const MotorPerifDef** motorMapSelect)
GPIO_WriteBit(motorMap[i]->gpioPowerswitchPort, motorMap[i]->gpioPowerswitchPin, 1);
}
+ // Configure the GPIO for CF-BL ESC RST
+ GPIO_StructInit(&GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;
+ GPIO_Init(GPIOC, &GPIO_InitStructure);
+
// Configure the GPIO for the timer output
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = MOTORS_GPIO_MODE;
@@ -340,7 +348,10 @@ void motorsStop()
}
#ifdef CONFIG_MOTORS_ESC_PROTOCOL_DSHOT
- motorsBurstDshot();
+ if (motorMap[0]->drvType == BRUSHLESS)
+ {
+ motorsBurstDshot();
+ }
#endif
}
@@ -467,7 +478,13 @@ void motorsSetRatio(uint32_t id, uint16_t ithrust)
uint16_t ratio = ithrust;
- if (motorSetEnable) {
+ // Override ratio in case of motorSetEnable
+ if (motorSetEnable == 2)
+ {
+ ratio = motorPowerSet[MOTOR_M1];
+ }
+ else if (motorSetEnable == 1)
+ {
ratio = motorPowerSet[id];
}
diff --git a/src/modules/src/crtp.c b/src/modules/src/crtp.c
index 401915f7c1..d2518eba4b 100644
--- a/src/modules/src/crtp.c
+++ b/src/modules/src/crtp.c
@@ -70,7 +70,7 @@ static struct {
static xQueueHandle txQueue;
#define CRTP_NBR_OF_PORTS 16
-#define CRTP_TX_QUEUE_SIZE 120
+#define CRTP_TX_QUEUE_SIZE 200
#define CRTP_RX_QUEUE_SIZE 16
static void crtpTxTask(void *param);
diff --git a/src/modules/src/log.c b/src/modules/src/log.c
index 3f7094a343..0442d61fdf 100644
--- a/src/modules/src/log.c
+++ b/src/modules/src/log.c
@@ -121,6 +121,9 @@ struct ops_setting_v2 {
uint16_t id;
} __attribute__((packed));
+struct control_start_block_v2 {
+ uint16_t period_in_ms;
+} __attribute__((packed));
#define TOC_CH 0
#define CONTROL_CH 1
@@ -139,6 +142,7 @@ struct ops_setting_v2 {
#define CONTROL_RESET 5
#define CONTROL_CREATE_BLOCK_V2 6
#define CONTROL_APPEND_BLOCK_V2 7
+#define CONTROL_START_BLOCK_V2 8
#define BLOCK_ID_FREE -1
@@ -420,6 +424,12 @@ void logControlProcess()
(struct ops_setting_v2*)&p.data[2],
(p.size-2)/sizeof(struct ops_setting_v2) );
break;
+ case CONTROL_START_BLOCK_V2:
+ {
+ struct control_start_block_v2* args = (struct control_start_block_v2 *)&p.data[2];
+ ret = logStartBlock(p.data[1], args->period_in_ms);
+ }
+ break;
}
//Commands answer
diff --git a/src/utils/src/configblockeeprom.c b/src/utils/src/configblockeeprom.c
index 950bd332b5..04fb0af510 100644
--- a/src/utils/src/configblockeeprom.c
+++ b/src/utils/src/configblockeeprom.c
@@ -124,10 +124,6 @@ int configblockInit(void)
eepromInit(I2C1_DEV);
- // Because of strange behavior from I2C device during expansion port test
- // the first read needs to be discarded
- eepromTestConnection();
-
if (eepromTestConnection())
{
if (eepromReadBuffer((uint8_t *)&configblock, 0, sizeof(configblock)))
diff --git a/tools/system_id/README.md b/tools/system_id/README.md
new file mode 100644
index 0000000000..e59dccc9dc
--- /dev/null
+++ b/tools/system_id/README.md
@@ -0,0 +1,67 @@
+# System ID including PWM, RPM, Thrust, Voltage, Current, and Power
+
+This folder contains scripts to measure propellers and motors using the systemId deck.
+
+## Setup
+
+* Load cells:
+ * 100g: https://www.sparkfun.com/products/14727
+ * 500g: https://www.sparkfun.com/products/14728
+ * 1kg: https://www.adafruit.com/product/4540
+* SystemID board with the following ICs
+ * NAU7802 (thrust)
+ * ACS37800 (power)
+ * QRD1114 (RPM)
+* Calibration weights
+* M2/M3 Nylon screws
+* CF Mount (3D printed)
+
+Mount CF upside down (to avoid downwash)
+
+## Measurement
+
+### Common
+
+1. Mount CF
+2. Switch to crazyflie-lib-python with branch `feature-emergency-stop` in order to be able to CTRL+C a script safely.
+3. Build firmware with configuration `make sysid_defconfig`
+4. Flash firmware
+5. Run `python3 calibscale.py --uri ` and follow the prompts to calibrate the load cell. This will create an output file `calibration.yaml` with the calibration data. The other scripts will read this file (other name can be specified as command line argument).
+
+### Ramping Motors
+
+This test will ramp the motors and write collected data to `data.csv`.
+
+```
+python3 collect_data.py --uri
+```
+
+The results can be visualized with `plot_data.py`.
+
+### Find (PWM,VBat)->Force, (desired Force,VBat)->PWM, (PWM, VBat)-> MaxForce
+
+This test will randomly select sample a PWM value and average the resulting force and measured vbat over a short period of time. Then, it will output the largest possible PWM value to estimate the maximum force that the Crazyflie can create at the current state-of-charge. The mapping functions can be automatically computed.
+
+```
+python3 collect_data_max_thrust.py --uri
+python3 system_id.py
+```
+
+### Motor Delay
+
+This test uses a higher sampling rate and visualizes the delayed motor response given a step input.
+
+```
+python3 collect_data_motor_delay.py --uri
+python3 plot_motor_delay.py
+```
+
+### Efficency
+
+This test will staircase the motors and write collected data to `data.csv`.
+
+```
+python3 collect_data_efficiency.py --uri
+```
+
+The results can be visualized with `plot_data_efficiency.py`.
diff --git a/tools/system_id/calibscale.py b/tools/system_id/calibscale.py
new file mode 100644
index 0000000000..d5bb8ea222
--- /dev/null
+++ b/tools/system_id/calibscale.py
@@ -0,0 +1,145 @@
+"""
+Calibrate load cell
+"""
+import argparse
+import logging
+import time
+import yaml
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+class CalibScale:
+ def __init__(self, link_uri):
+ """ Initialize and run with the specified link_uri """
+
+ self.measurements = []
+
+ self._cf = Crazyflie(rw_cache='./cache')
+
+ # Connect some callbacks from the Crazyflie API
+ self._cf.connected.add_callback(self._connected)
+ self._cf.disconnected.add_callback(self._disconnected)
+ self._cf.connection_failed.add_callback(self._connection_failed)
+ self._cf.connection_lost.add_callback(self._connection_lost)
+
+ print('Connecting to %s' % link_uri)
+
+ # Try to connect to the Crazyflie
+ self._cf.open_link(link_uri)
+
+ # Variable used to keep main loop occupied until disconnect
+ self.is_connected = False
+
+ def _connected(self, link_uri):
+ """ This callback is called form the Crazyflie API when a Crazyflie
+ has been connected and the TOCs have been downloaded."""
+ print('Connected to %s' % link_uri)
+
+ # The definition of the logconfig can be made before connecting
+ self._lg_stab = LogConfig(name='data', period_in_ms=10)
+ self._lg_stab.add_variable('loadcell.weight', 'float')
+ self._lg_stab.add_variable('loadcell.rawWeight', 'int32_t')
+
+ # Adding the configuration cannot be done until a Crazyflie is
+ # connected, since we need to check that the variables we
+ # would like to log are in the TOC.
+ try:
+ self._cf.log.add_config(self._lg_stab)
+ # This callback will receive the data
+ self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
+ # This callback will be called on errors
+ self._lg_stab.error_cb.add_callback(self._stab_log_error)
+ # Start the logging
+ self._lg_stab.start()
+ except KeyError as e:
+ print('Could not start log configuration,'
+ '{} not found in TOC'.format(str(e)))
+ except AttributeError:
+ print('Could not add Stabilizer log config, bad configuration.')
+
+ self.is_connected = True
+
+ def _stab_log_error(self, logconf, msg):
+ """Callback from the log API when an error occurs"""
+ print('Error when logging %s: %s' % (logconf.name, msg))
+
+ def _stab_log_data(self, timestamp, data, logconf):
+ """Callback froma the log API when data arrives"""
+ # print('[%d][%s]: %s' % (timestamp, logconf.name, data))
+ self.measurements.append(data['loadcell.rawWeight'])
+
+ def _connection_failed(self, link_uri, msg):
+ """Callback when connection initial connection fails (i.e no Crazyflie
+ at the speficied address)"""
+ print('Connection to %s failed: %s' % (link_uri, msg))
+ self.is_connected = False
+
+ def _connection_lost(self, link_uri, msg):
+ """Callback when disconnected after a connection has been made (i.e
+ Crazyflie moves out of range)"""
+ print('Connection to %s lost: %s' % (link_uri, msg))
+
+ def _disconnected(self, link_uri):
+ """Callback when the Crazyflie is disconnected (called in all cases)"""
+ print('Disconnected from %s' % link_uri)
+ self.is_connected = False
+
+ def measure(self, num_measurements = 100):
+ weights = []
+ readings = []
+
+ while True:
+ data = float(input("enter weight in grams (-1 to end): "))
+ if data < 0:
+ break
+ self.measurements = []
+ while len(self.measurements) < num_measurements:
+ time.sleep(0.1)
+ print(np.mean(self.measurements))
+ weights.append(data)
+ readings.append(np.mean(self.measurements))
+
+ self._cf.close_link()
+
+ z = np.polyfit(readings, weights, 1)
+ p = np.poly1d(z)
+
+ xp = np.linspace(readings[0], readings[-1], 100)
+
+
+ plt.plot(readings, weights, '.', xp, p(xp), '--')
+ plt.show()
+ return float(z[0]), float(z[1])
+
+
+if __name__ == '__main__':
+ parser = argparse.ArgumentParser()
+ parser.add_argument("--output", default="calibration.yaml", help="Output file containing the calibration")
+ parser.add_argument("--uri", default="radio://0/42/2M/E7E7E7E7E7", help="URI of Crazyflie")
+ args = parser.parse_args()
+
+ # Only output errors from the logging framework
+ logging.basicConfig(level=logging.ERROR)
+
+ # Initialize the low-level drivers (don't list the debug drivers)
+ cflib.crtp.init_drivers(enable_debug_driver=False)
+ le = CalibScale(args.uri)
+
+ while not le.is_connected:
+ time.sleep(0.1)
+
+ a,b = le.measure()
+ result = dict()
+ result['a'] = a
+ result['b'] = b
+
+ with open(args.output, 'w') as yaml_file:
+ yaml.dump(result, yaml_file)
+
+
diff --git a/tools/system_id/collect_data.py b/tools/system_id/collect_data.py
new file mode 100644
index 0000000000..b23742573f
--- /dev/null
+++ b/tools/system_id/collect_data.py
@@ -0,0 +1,188 @@
+"""
+Ramp motors and collect data using the system-id deck
+"""
+import argparse
+import logging
+import time
+import yaml
+from threading import Thread
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+from cflib.crazyflie.localization import Localization
+
+
+class CollectData:
+ """
+ Simple logging example class that logs the Stabilizer from a supplied
+ link uri and disconnects after 5s.
+ """
+
+ def __init__(self, link_uri, calib_a, calib_b):
+ """ Initialize and run the example with the specified link_uri """
+ self.calib_a = calib_a
+ self.calib_b = calib_b
+
+ self._cf = Crazyflie(rw_cache='./cache')
+
+ # Connect some callbacks from the Crazyflie API
+ self._cf.connected.add_callback(self._connected)
+ self._cf.disconnected.add_callback(self._disconnected)
+ self._cf.connection_failed.add_callback(self._connection_failed)
+ self._cf.connection_lost.add_callback(self._connection_lost)
+
+ print('Connecting to %s' % link_uri)
+
+ # Try to connect to the Crazyflie
+ self._cf.open_link(link_uri)
+
+ # Variable used to keep main loop occupied until disconnect
+ self.is_connected = True
+
+ def _connected(self, link_uri):
+ """ This callback is called form the Crazyflie API when a Crazyflie
+ has been connected and the TOCs have been downloaded."""
+ print('Connected to %s' % link_uri)
+
+ print(self.calib_a, self.calib_b)
+ self._cf.param.set_value('loadcell.a', str(self.calib_a))
+ self._cf.param.set_value('loadcell.b', str(self.calib_b))
+
+ self._file = open("data.csv", "w+")
+ self._file.write("weight[g],pwm,vbat[V],rpm1,rpm2,rpm3,rpm4,v[V],i[A],p[W]\n");
+
+ # The definition of the logconfig can be made before connecting
+ self._lg_stab = LogConfig(name='data', period_in_ms=10)
+ self._lg_stab.add_variable('loadcell.weight', 'float')
+ self._lg_stab.add_variable('pwm.m1_pwm', 'uint16_t')
+ self._lg_stab.add_variable('pm.vbatMV', 'uint16_t')
+ self._lg_stab.add_variable('rpm.m1', 'uint16_t')
+ self._lg_stab.add_variable('rpm.m2', 'uint16_t')
+ self._lg_stab.add_variable('rpm.m3', 'uint16_t')
+ self._lg_stab.add_variable('rpm.m4', 'uint16_t')
+ self._lg_stab.add_variable('asc37800.v_mV', 'int16_t')
+ self._lg_stab.add_variable('asc37800.i_mA', 'int16_t')
+ self._lg_stab.add_variable('asc37800.p_mW', 'int16_t')
+
+ # Adding the configuration cannot be done until a Crazyflie is
+ # connected, since we need to check that the variables we
+ # would like to log are in the TOC.
+ try:
+ self._cf.log.add_config(self._lg_stab)
+ # This callback will receive the data
+ self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
+ # This callback will be called on errors
+ self._lg_stab.error_cb.add_callback(self._stab_log_error)
+ # Start the logging
+ self._lg_stab.start()
+ except KeyError as e:
+ print('Could not start log configuration,'
+ '{} not found in TOC'.format(str(e)))
+ except AttributeError:
+ print('Could not add Stabilizer log config, bad configuration.')
+
+ # Start a separate thread to do the motor test.
+ # Do not hijack the calling thread!
+ Thread(target=self._ramp_motors).start()
+
+ def _stab_log_error(self, logconf, msg):
+ """Callback from the log API when an error occurs"""
+ print('Error when logging %s: %s' % (logconf.name, msg))
+
+ def _stab_log_data(self, timestamp, data, logconf):
+ """Callback froma the log API when data arrives"""
+ print('[%d][%s]: %s' % (timestamp, logconf.name, data))
+ self._file.write("{},{},{},{},{},{},{},{},{},{}\n".format(
+ data['loadcell.weight'],
+ data['pwm.m1_pwm'],
+ data['pm.vbatMV']/ 1000,
+ data['rpm.m1'],
+ data['rpm.m2'],
+ data['rpm.m3'],
+ data['rpm.m4'],
+ data['asc37800.v_mV']/ 1000,
+ data['asc37800.i_mA']/ 1000,
+ data['asc37800.p_mW']/ 1000))
+
+ def _connection_failed(self, link_uri, msg):
+ """Callback when connection initial connection fails (i.e no Crazyflie
+ at the speficied address)"""
+ print('Connection to %s failed: %s' % (link_uri, msg))
+ self.is_connected = False
+
+ def _connection_lost(self, link_uri, msg):
+ """Callback when disconnected after a connection has been made (i.e
+ Crazyflie moves out of range)"""
+ print('Connection to %s lost: %s' % (link_uri, msg))
+
+ def _disconnected(self, link_uri):
+ """Callback when the Crazyflie is disconnected (called in all cases)"""
+ print('Disconnected from %s' % link_uri)
+ self.is_connected = False
+
+ def _ramp_motors(self):
+ thrust_mult = 1
+ thrust_step = 500
+ time_step = 0.1
+ thrust = 0
+ pitch = 0
+ roll = 0
+ yawrate = 0
+
+ # # Unlock startup thrust protection
+ # for i in range(0, 100):
+ # self._cf.commander.send_setpoint(0, 0, 0, 0)
+
+ localization = Localization(self._cf)
+
+ self._cf.param.set_value('motor.batCompensation', 0)
+ self._cf.param.set_value('motorPowerSet.m1', 0)
+ self._cf.param.set_value('motorPowerSet.enable', 2)
+ self._cf.param.set_value('system.forceArm', 1)
+
+ while self.is_connected: #thrust >= 0:
+ thrust += thrust_step * thrust_mult
+ if thrust >= 65536 or thrust < 0:
+ # if thrust >= 20000 or thrust < 0:
+ thrust_mult *= -1
+ thrust += thrust_step * thrust_mult
+ print(thrust)
+ # self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
+ localization.send_emergency_stop_watchdog()
+ self._cf.param.set_value('motorPowerSet.m1', str(thrust))
+ time.sleep(time_step)
+
+ # self._cf.commander.send_setpoint(0, 0, 0, 0)
+ # Make sure that the last packet leaves before the link is closed
+ # since the message queue is not flushed before closing
+ # time.sleep(0.1)
+ # self._cf.close_link()
+
+
+if __name__ == '__main__':
+
+ parser = argparse.ArgumentParser()
+ parser.add_argument("--calibration", default="calibration.yaml", help="Input file containing the calibration")
+ parser.add_argument("--uri", default="radio://0/42/2M/E7E7E7E7E7", help="URI of Crazyflie")
+ args = parser.parse_args()
+
+ # Only output errors from the logging framework
+ logging.basicConfig(level=logging.ERROR)
+
+ # Initialize the low-level drivers (don't list the debug drivers)
+ cflib.crtp.init_drivers(enable_debug_driver=False)
+
+ with open(args.calibration, 'r') as f:
+ r = yaml.safe_load(f)
+ a = r['a']
+ b = r['b']
+
+ # collect data
+ le = CollectData(args.uri, a, b)
+
+ # The Crazyflie lib doesn't contain anything to keep the application alive,
+ # so this is where your application should do something. In our case we
+ # are just waiting until we are disconnected.
+ while le.is_connected:
+ time.sleep(1)
diff --git a/tools/system_id/collect_data_efficiency.py b/tools/system_id/collect_data_efficiency.py
new file mode 100644
index 0000000000..5a80a2c1f6
--- /dev/null
+++ b/tools/system_id/collect_data_efficiency.py
@@ -0,0 +1,180 @@
+"""
+Ramp motors and collect data using the system-id deck
+"""
+import argparse
+import logging
+import time
+import yaml
+from threading import Thread
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+from cflib.crazyflie.localization import Localization
+
+
+class CollectData:
+ """
+ Simple logging example class that logs the Stabilizer from a supplied
+ link uri and disconnects after 5s.
+ """
+
+ def __init__(self, link_uri, calib_a, calib_b):
+ """ Initialize and run the example with the specified link_uri """
+ self.calib_a = calib_a
+ self.calib_b = calib_b
+
+ self._cf = Crazyflie(rw_cache='./cache')
+
+ # Connect some callbacks from the Crazyflie API
+ self._cf.fully_connected.add_callback(self._connected)
+ self._cf.disconnected.add_callback(self._disconnected)
+ self._cf.connection_failed.add_callback(self._connection_failed)
+ self._cf.connection_lost.add_callback(self._connection_lost)
+
+ print('Connecting to %s' % link_uri)
+
+ # Try to connect to the Crazyflie
+ self._cf.open_link(link_uri)
+
+ # Variable used to keep main loop occupied until disconnect
+ self.is_connected = True
+
+ def _connected(self, link_uri):
+ """ This callback is called form the Crazyflie API when a Crazyflie
+ has been connected and the TOCs have been downloaded."""
+ print('Connected to %s' % link_uri)
+
+ print(self.calib_a, self.calib_b)
+ self._cf.param.set_value('loadcell.a', str(self.calib_a))
+ self._cf.param.set_value('loadcell.b', str(self.calib_b))
+
+ self._file = open("data.csv", "w+")
+ self._file.write("weight[g],pwm,vbat[V],rpm1,rpm2,rpm3,rpm4,v[V],i[A],p[W]\n");
+
+ # The definition of the logconfig can be made before connecting
+ self._lg_stab = LogConfig(name='data', period_in_ms=10)
+ self._lg_stab.add_variable('loadcell.weight', 'float')
+ self._lg_stab.add_variable('motor.m1', 'uint16_t')
+ self._lg_stab.add_variable('pm.vbatMV', 'uint16_t')
+ self._lg_stab.add_variable('rpm.m1', 'uint16_t')
+ self._lg_stab.add_variable('rpm.m2', 'uint16_t')
+ self._lg_stab.add_variable('rpm.m3', 'uint16_t')
+ self._lg_stab.add_variable('rpm.m4', 'uint16_t')
+ self._lg_stab.add_variable('asc37800.v_mV', 'int16_t')
+ self._lg_stab.add_variable('asc37800.i_mA', 'int16_t')
+ self._lg_stab.add_variable('asc37800.p_mW', 'int16_t')
+
+ # Adding the configuration cannot be done until a Crazyflie is
+ # connected, since we need to check that the variables we
+ # would like to log are in the TOC.
+ try:
+ self._cf.log.add_config(self._lg_stab)
+ # This callback will receive the data
+ self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
+ # This callback will be called on errors
+ self._lg_stab.error_cb.add_callback(self._stab_log_error)
+ # Start the logging
+ self._lg_stab.start()
+ except KeyError as e:
+ print('Could not start log configuration,'
+ '{} not found in TOC'.format(str(e)))
+ except AttributeError:
+ print('Could not add Stabilizer log config, bad configuration.')
+
+ # Start a separate thread to do the motor test.
+ # Do not hijack the calling thread!
+ Thread(target=self._ramp_motors).start()
+
+ def _stab_log_error(self, logconf, msg):
+ """Callback from the log API when an error occurs"""
+ print('Error when logging %s: %s' % (logconf.name, msg))
+
+ def _stab_log_data(self, timestamp, data, logconf):
+ """Callback froma the log API when data arrives"""
+ print('[%d][%s]: %s' % (timestamp, logconf.name, data))
+ self._file.write("{},{},{},{},{},{},{},{},{},{}\n".format(
+ data['loadcell.weight'],
+ data['motor.m1'],
+ data['pm.vbatMV']/ 1000,
+ data['rpm.m1'],
+ data['rpm.m2'],
+ data['rpm.m3'],
+ data['rpm.m4'],
+ data['asc37800.v_mV']/ 1000,
+ data['asc37800.i_mA']/ 1000,
+ data['asc37800.p_mW']/ 1000))
+
+ def _connection_failed(self, link_uri, msg):
+ """Callback when connection initial connection fails (i.e no Crazyflie
+ at the speficied address)"""
+ print('Connection to %s failed: %s' % (link_uri, msg))
+ self.is_connected = False
+
+ def _connection_lost(self, link_uri, msg):
+ """Callback when disconnected after a connection has been made (i.e
+ Crazyflie moves out of range)"""
+ print('Connection to %s lost: %s' % (link_uri, msg))
+
+ def _disconnected(self, link_uri):
+ """Callback when the Crazyflie is disconnected (called in all cases)"""
+ print('Disconnected from %s' % link_uri)
+ self.is_connected = False
+
+ def _ramp_motors(self):
+ thrust_mult = 1
+ thrust_steps = 10
+ time_step = 1.0
+ thrust = 0
+ pitch = 0
+ roll = 0
+ yawrate = 0
+
+ # # Unlock startup thrust protection
+ # for i in range(0, 100):
+ # self._cf.commander.send_setpoint(0, 0, 0, 0)
+
+ localization = Localization(self._cf)
+
+ self._cf.param.set_value('motorPowerSet.m1', 0)
+ self._cf.param.set_value('motorPowerSet.enable', 2)
+
+ for i in range(10):
+ thrust = int((65536/10)*i)
+ localization.send_emergency_stop_watchdog()
+ self._cf.param.set_value('motorPowerSet.m1', str(thrust))
+ time.sleep(time_step)
+
+ # self._cf.commander.send_setpoint(0, 0, 0, 0)
+ # Make sure that the last packet leaves before the link is closed
+ # since the message queue is not flushed before closing
+ # time.sleep(0.1)
+ self._cf.close_link()
+
+
+if __name__ == '__main__':
+
+ parser = argparse.ArgumentParser()
+ parser.add_argument("--calibration", default="calibration.yaml", help="Input file containing the calibration")
+ parser.add_argument("--uri", default="radio://0/42/2M/E7E7E7E7E7", help="URI of Crazyflie")
+ args = parser.parse_args()
+
+ # Only output errors from the logging framework
+ logging.basicConfig(level=logging.ERROR)
+
+ # Initialize the low-level drivers (don't list the debug drivers)
+ cflib.crtp.init_drivers(enable_debug_driver=False)
+
+ with open(args.calibration, 'r') as f:
+ r = yaml.safe_load(f)
+ a = r['a']
+ b = r['b']
+
+ # collect data
+ le = CollectData(args.uri, a, b)
+
+ # The Crazyflie lib doesn't contain anything to keep the application alive,
+ # so this is where your application should do something. In our case we
+ # are just waiting until we are disconnected.
+ while le.is_connected:
+ time.sleep(1)
diff --git a/tools/system_id/collect_data_max_thrust.py b/tools/system_id/collect_data_max_thrust.py
new file mode 100644
index 0000000000..3b6b6fdb6b
--- /dev/null
+++ b/tools/system_id/collect_data_max_thrust.py
@@ -0,0 +1,201 @@
+"""
+Example that uses a load cell to measure thrust using different RPMs
+"""
+import argparse
+import logging
+import time
+from threading import Thread
+import numpy as np
+import yaml
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+from cflib.crazyflie.localization import Localization
+
+
+# Only output errors from the logging framework
+logging.basicConfig(level=logging.ERROR)
+
+
+class CollectData:
+ """
+ Simple logging example class that logs the Stabilizer from a supplied
+ link uri and disconnects after 5s.
+ """
+
+ def __init__(self, link_uri, calib_a, calib_b):
+ """ Initialize and run the example with the specified link_uri """
+ self.measurements = []
+ self.desiredThrust = 0
+ self.calib_a = calib_a
+ self.calib_b = calib_b
+
+ self._cf = Crazyflie(rw_cache='./cache')
+
+ # Connect some callbacks from the Crazyflie API
+ self._cf.connected.add_callback(self._connected)
+ self._cf.disconnected.add_callback(self._disconnected)
+ self._cf.connection_failed.add_callback(self._connection_failed)
+ self._cf.connection_lost.add_callback(self._connection_lost)
+
+ print('Connecting to %s' % link_uri)
+
+ # Try to connect to the Crazyflie
+ self._cf.open_link(link_uri)
+
+ # Variable used to keep main loop occupied until disconnect
+ self.is_connected = True
+
+ def _connected(self, link_uri):
+ """ This callback is called form the Crazyflie API when a Crazyflie
+ has been connected and the TOCs have been downloaded."""
+ print('Connected to %s' % link_uri)
+
+ print(self.calib_a, self.calib_b)
+ self._cf.param.set_value('loadcell.a', str(self.calib_a))
+ self._cf.param.set_value('loadcell.b', str(self.calib_b))
+
+ self._file = open("data.csv", "w+")
+ self._file.write("thrust[g],pwm,vbat[V],maxThrust[g],maxThrustVbat[V],rpm\n");
+
+ # The definition of the logconfig can be made before connecting
+ self._lg_stab = LogConfig(name='data', period_in_ms=10)
+ self._lg_stab.add_variable('loadcell.weight', 'float')
+ self._lg_stab.add_variable('motor.m1', 'uint16_t')
+ self._lg_stab.add_variable('pm.vbat', 'float')
+ self._lg_stab.add_variable('rpm.m1', 'uint16_t')
+ self._lg_stab.add_variable('rpm.m2', 'uint16_t')
+ self._lg_stab.add_variable('rpm.m3', 'uint16_t')
+ self._lg_stab.add_variable('rpm.m4', 'uint16_t')
+
+ # Adding the configuration cannot be done until a Crazyflie is
+ # connected, since we need to check that the variables we
+ # would like to log are in the TOC.
+ try:
+ self._cf.log.add_config(self._lg_stab)
+ # This callback will receive the data
+ self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
+ # This callback will be called on errors
+ self._lg_stab.error_cb.add_callback(self._stab_log_error)
+ # Start the logging
+ self._lg_stab.start()
+ except KeyError as e:
+ print('Could not start log configuration,'
+ '{} not found in TOC'.format(str(e)))
+ except AttributeError:
+ print('Could not add Stabilizer log config, bad configuration.')
+
+ self._localization = Localization(self._cf)
+
+ # Start a separate thread to do the motor test.
+ # Do not hijack the calling thread!
+ Thread(target=self._ramp_motors).start()
+
+ # # Start a timer to disconnect in 10s
+ # t = Timer(5, self._cf.close_link)
+ # t.start()
+
+ def _stab_log_error(self, logconf, msg):
+ """Callback from the log API when an error occurs"""
+ print('Error when logging %s: %s' % (logconf.name, msg))
+
+ def _stab_log_data(self, timestamp, data, logconf):
+ """Callback froma the log API when data arrives"""
+ # print('[%d][%s]: %s' % (timestamp, logconf.name, data))
+ # self._file.write("{},{},{}\n".format(data['loadcell.weight'], data['motor.m1'], data['pm.vbat']))
+ if self.desiredThrust == data['motor.m1']:
+ self.measurements.append(np.array([data['loadcell.weight'], data['pm.vbat']]))
+ # pass
+
+ def _connection_failed(self, link_uri, msg):
+ """Callback when connection initial connection fails (i.e no Crazyflie
+ at the speficied address)"""
+ print('Connection to %s failed: %s' % (link_uri, msg))
+ self.is_connected = False
+
+ def _connection_lost(self, link_uri, msg):
+ """Callback when disconnected after a connection has been made (i.e
+ Crazyflie moves out of range)"""
+ print('Connection to %s lost: %s' % (link_uri, msg))
+
+ def _disconnected(self, link_uri):
+ """Callback when the Crazyflie is disconnected (called in all cases)"""
+ print('Disconnected from %s' % link_uri)
+ self.is_connected = False
+
+ def _measure(self, thrust, min_samples = 50):
+ self.desiredThrust = thrust
+ self.measurements = []
+ self._cf.param.set_value('motorPowerSet.m1', str(thrust))
+ while len(self.measurements) < min_samples:
+ self._localization.send_emergency_stop_watchdog()
+ time.sleep(0.1)
+ m = np.array(self.measurements)
+ # only return the last few samples
+ return m[40:]
+
+ def _ramp_motors(self):
+
+ self._cf.param.set_value('motorPowerSet.m1', 0)
+ self._cf.param.set_value('motorPowerSet.enable', 2)
+
+ while self.is_connected: #thrust >= 0:
+ # randomply sample PWM
+ thrust = int(np.random.uniform(15000, 65535))
+ measurements = self._measure(thrust)
+ # print(measurements)
+ achievedThrust = np.mean(measurements[:,0])
+ vbat = np.mean(measurements[:,1])
+
+ # go to full thrust
+ measurements = self._measure(65535)
+ # print(measurements)
+ maxThrust = np.mean(measurements[:,0])
+ vbatAtMaxThrust = np.mean(measurements[:,1])
+
+ # write result
+ print(achievedThrust, thrust, vbat, maxThrust, vbatAtMaxThrust)
+ self._file.write("{},{},{},{},{}\n".format(
+ achievedThrust,
+ thrust,
+ vbat,
+ maxThrust,
+ vbatAtMaxThrust))
+
+ if vbatAtMaxThrust < 2.8:
+ break
+
+ # self._cf.commander.send_setpoint(0, 0, 0, 0)
+ # Make sure that the last packet leaves before the link is closed
+ # since the message queue is not flushed before closing
+ time.sleep(0.1)
+ self._cf.close_link()
+ self._file.close()
+
+
+if __name__ == '__main__':
+ parser = argparse.ArgumentParser()
+ parser.add_argument("--calibration", default="calibration.yaml", help="Input file containing the calibration")
+ parser.add_argument("--uri", default="radio://0/42/2M/E7E7E7E7E7", help="URI of Crazyflie")
+ args = parser.parse_args()
+
+ # Only output errors from the logging framework
+ logging.basicConfig(level=logging.ERROR)
+
+ # Initialize the low-level drivers (don't list the debug drivers)
+ cflib.crtp.init_drivers(enable_debug_driver=False)
+
+ with open(args.calibration, 'r') as f:
+ r = yaml.safe_load(f)
+ a = r['a']
+ b = r['b']
+
+ # collect data
+ le = CollectData(args.uri, a, b)
+
+ # The Crazyflie lib doesn't contain anything to keep the application alive,
+ # so this is where your application should do something. In our case we
+ # are just waiting until we are disconnected.
+ while le.is_connected:
+ time.sleep(1)
diff --git a/tools/system_id/collect_data_motor_delay.py b/tools/system_id/collect_data_motor_delay.py
new file mode 100644
index 0000000000..dce0d3fee4
--- /dev/null
+++ b/tools/system_id/collect_data_motor_delay.py
@@ -0,0 +1,192 @@
+"""
+Example that uses a load cell to measure thrust using different RPMs
+"""
+import argparse
+import logging
+import time
+from threading import Thread
+import yaml
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+from cflib.crazyflie.localization import Localization
+
+
+# Only output errors from the logging framework
+logging.basicConfig(level=logging.ERROR)
+
+
+class CollectData:
+ """
+ Simple logging example class that logs the Stabilizer from a supplied
+ link uri and disconnects after 5s.
+ """
+
+ def __init__(self, link_uri, calib_a, calib_b):
+ """ Initialize and run the example with the specified link_uri """
+ self.calib_a = calib_a
+ self.calib_b = calib_b
+
+ self._cf = Crazyflie(rw_cache='./cache')
+
+ # Connect some callbacks from the Crazyflie API
+ self._cf.connected.add_callback(self._connected)
+ self._cf.disconnected.add_callback(self._disconnected)
+ self._cf.connection_failed.add_callback(self._connection_failed)
+ self._cf.connection_lost.add_callback(self._connection_lost)
+
+ print('Connecting to %s' % link_uri)
+
+ # Try to connect to the Crazyflie
+ self._cf.open_link(link_uri)
+
+ # Variable used to keep main loop occupied until disconnect
+ self.is_connected = True
+
+ def _connected(self, link_uri):
+ """ This callback is called form the Crazyflie API when a Crazyflie
+ has been connected and the TOCs have been downloaded."""
+ print('Connected to %s' % link_uri)
+
+ print(self.calib_a, self.calib_b)
+ self._cf.param.set_value('loadcell.a', str(self.calib_a))
+ self._cf.param.set_value('loadcell.b', str(self.calib_b))
+ self._cf.param.set_value('loadcell.sampleRate', 7) # fastest sample rate: 320 Hz;
+ # self._cf.param.set_value('loadcell.sampleRate', 4)
+
+ self._file = open("data.csv", "w+")
+ self._file.write("time[ms],weight[g],pwm,vbat[V],rpm1,rpm2,rpm3,rpm4,v[V],i[A]\n");
+
+ # The definition of the logconfig can be made before connecting
+ self._lg_stab = LogConfig(name='data', period_in_ms=5)
+ self._lg_stab.add_variable('loadcell.weight', 'float')
+ self._lg_stab.add_variable('motor.m1', 'uint16_t')
+ self._lg_stab.add_variable('pm.vbatMV', 'uint16_t')
+ self._lg_stab.add_variable('rpm.m1', 'uint16_t')
+ self._lg_stab.add_variable('rpm.m2', 'uint16_t')
+ self._lg_stab.add_variable('rpm.m3', 'uint16_t')
+ self._lg_stab.add_variable('rpm.m4', 'uint16_t')
+ self._lg_stab.add_variable('asc37800.v_avg', 'float')
+ self._lg_stab.add_variable('asc37800.i_avg', 'float')
+
+ # Adding the configuration cannot be done until a Crazyflie is
+ # connected, since we need to check that the variables we
+ # would like to log are in the TOC.
+ try:
+ self._cf.log.add_config(self._lg_stab)
+ # This callback will receive the data
+ self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
+ # This callback will be called on errors
+ self._lg_stab.error_cb.add_callback(self._stab_log_error)
+ # Start the logging
+ self._lg_stab.start_v2() # Use special v2 version to be able to sample > 100 Hz
+ except KeyError as e:
+ print('Could not start log configuration,'
+ '{} not found in TOC'.format(str(e)))
+ except AttributeError:
+ print('Could not add Stabilizer log config, bad configuration.')
+
+ self._localization = Localization(self._cf)
+
+ # Start a separate thread to do the motor test.
+ # Do not hijack the calling thread!
+ Thread(target=self._ramp_motors).start()
+
+ # # Start a timer to disconnect in 10s
+ # t = Timer(5, self._cf.close_link)
+ # t.start()
+
+ def _stab_log_error(self, logconf, msg):
+ """Callback from the log API when an error occurs"""
+ print('Error when logging %s: %s' % (logconf.name, msg))
+
+ def _stab_log_data(self, timestamp, data, logconf):
+ """Callback froma the log API when data arrives"""
+ print('[%d][%s]: %s' % (timestamp, logconf.name, data))
+ self._file.write("{},{},{},{},{},{},{},{},{},{}\n".format(
+ timestamp,
+ data['loadcell.weight'],
+ data['motor.m1'],
+ data['pm.vbatMV'] / 1000,
+ data['rpm.m1'],
+ data['rpm.m2'],
+ data['rpm.m3'],
+ data['rpm.m4'],
+ data['asc37800.v_avg'],
+ data['asc37800.i_avg']))
+
+ def _connection_failed(self, link_uri, msg):
+ """Callback when connection initial connection fails (i.e no Crazyflie
+ at the speficied address)"""
+ print('Connection to %s failed: %s' % (link_uri, msg))
+ self.is_connected = False
+
+ def _connection_lost(self, link_uri, msg):
+ """Callback when disconnected after a connection has been made (i.e
+ Crazyflie moves out of range)"""
+ print('Connection to %s lost: %s' % (link_uri, msg))
+
+ def _disconnected(self, link_uri):
+ """Callback when the Crazyflie is disconnected (called in all cases)"""
+ print('Disconnected from %s' % link_uri)
+ self.is_connected = False
+
+ def _applyThrust(self, thrust, duration):
+ start = time.time()
+ self._cf.param.set_value('motorPowerSet.m1', int(thrust))
+ while time.time() - start < duration:
+ self._localization.send_emergency_stop_watchdog()
+ print(time.time() - start)
+ time.sleep(0.1)
+
+ def _ramp_motors(self):
+ self._cf.param.set_value('motorPowerSet.m1', 0)
+ self._cf.param.set_value('motorPowerSet.enable', 2)
+
+ while self.is_connected:
+ # base speed
+ self._applyThrust(10000, 3.0)
+
+ # 0 -> 1
+ self._applyThrust(1.0 * 65535, 3.0)
+ # 1 -> 0
+ self._applyThrust(10000, 3.0)
+
+ # 0 -> 0.5
+ self._applyThrust(0.5 * 65535, 3.0)
+ # 0.5 -> 0
+ self._applyThrust(10000, 3.0)
+
+ break
+
+ self._cf.param.set_value('motorPowerSet.enable', 0)
+ time.sleep(1)
+ self._cf.close_link()
+
+
+if __name__ == '__main__':
+ parser = argparse.ArgumentParser()
+ parser.add_argument("--calibration", default="calibration.yaml", help="Input file containing the calibration")
+ parser.add_argument("--uri", default="radio://0/42/2M/E7E7E7E7E7", help="URI of Crazyflie")
+ args = parser.parse_args()
+
+ # Only output errors from the logging framework
+ logging.basicConfig(level=logging.ERROR)
+
+ # Initialize the low-level drivers (don't list the debug drivers)
+ cflib.crtp.init_drivers(enable_debug_driver=False)
+
+ with open(args.calibration, 'r') as f:
+ r = yaml.safe_load(f)
+ a = r['a']
+ b = r['b']
+
+ # collect data
+ le = CollectData(args.uri, a, b)
+
+ # The Crazyflie lib doesn't contain anything to keep the application alive,
+ # so this is where your application should do something. In our case we
+ # are just waiting until we are disconnected.
+ while le.is_connected:
+ time.sleep(1)
diff --git a/tools/system_id/plot_data.py b/tools/system_id/plot_data.py
new file mode 100644
index 0000000000..d106d9c8dc
--- /dev/null
+++ b/tools/system_id/plot_data.py
@@ -0,0 +1,48 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+def loadFile(filename):
+ fileData = np.loadtxt(filename, delimiter=',', skiprows=1, ndmin=2)
+ return fileData
+
+if __name__ == '__main__':
+ data = loadFile("data.csv")
+
+ thrust = data[:,0] / 4 # g, per motor
+ pwm = data[:,1] # PWM value
+ vbat = data[:,2] # V, battery voltage,
+ rpm = np.mean(data[:,3:7],axis=1) # average over all motors
+
+ fig,ax = plt.subplots(2)
+
+ ax[0].plot(data[:,4] - data[:,3], label='M2-M1')
+ ax[0].plot(data[:,5] - data[:,3], label='M3-M1')
+ ax[0].plot(data[:,6] - data[:,3], label='M4-M1')
+ ax[0].set_xlabel('Time')
+ ax[0].set_ylabel('Relative RPM')
+ ax[0].legend()
+
+ ax[1].plot(rpm, thrust)
+ ax[1].set_xlabel('RPM')
+ ax[1].set_ylabel('Thrust [g]')
+
+ z = np.polyfit(rpm, thrust, 2)
+ p = np.poly1d(z)
+
+ xp = np.linspace(np.min(rpm), np.max(rpm), 100)
+
+ ax[1].plot(xp, p(xp), '--')
+ ax[1].set_title("thrust [g] = {} * rpm^2 + {} * rpm + {}".format(z[0], z[1], z[2]))
+
+ plt.show()
+
+ fig, ax = plt.subplots()
+
+ print(data[0, 7]/vbat[0])
+
+ # ax.plot(thrust, vbat, label='pm.vbat')
+ # ax.plot(thrust, data[:, 7]/1.0955852742562724, label='i2c.v_avg')
+ # ax.plot(rpm, data[:, 8], label='i2c.i_avg')
+ ax.plot(thrust, data[:, 7] * data[:, 8], label='i2c.p_avg')
+ ax.legend()
+ plt.show()
diff --git a/tools/system_id/plot_data_efficiency.py b/tools/system_id/plot_data_efficiency.py
new file mode 100644
index 0000000000..1a1e97cf45
--- /dev/null
+++ b/tools/system_id/plot_data_efficiency.py
@@ -0,0 +1,66 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import argparse
+
+def loadFile(filename):
+ fileData = np.loadtxt(filename, delimiter=',', skiprows=1, ndmin=2)
+ return fileData
+
+if __name__ == '__main__':
+ parser = argparse.ArgumentParser()
+ parser.add_argument("filename")
+ args = parser.parse_args()
+ data = loadFile(args.filename)
+
+ thrust = data[:,0] / 4 # g, per motor
+ pwm = data[:,1] # PWM value
+ vbat = data[:,2] # V, battery voltage,
+ rpm = np.mean(data[:,3:7],axis=1) # average over all motors
+ vSid = data[:, 7] # Volts at system id deck
+ amp = data[:, 8] # Amps
+ pwr = data[:, 9] # Power in watts
+
+
+ fig, ax = plt.subplots(3)
+
+ print(vSid[0]/vbat[0])
+
+ ax[0].plot(data[:,3], label='M1')
+ ax[0].plot(data[:,4], label='M2')
+ ax[0].plot(data[:,5], label='M3')
+ ax[0].plot(data[:,6], label='M4')
+ ax[0].plot(rpm, label='Mean')
+ ax[0].set_xlabel('Time')
+ ax[0].set_ylabel('RPM')
+ ax[0].legend()
+ ax[0].grid(True)
+
+ ax2 = ax[0].twinx()
+ ax2.plot((pwr), label='P[W]')
+ ax2.plot(pwm/6553.5, color='tab:red', label='PWM[0-10]')
+ ax2.plot(thrust/((pwr / 4)), label='Efficency [g/W]')
+ ax2.set_ylabel('PWM[0-10], Power[W], Efficiency[g/W]', color='tab:red')
+ ax2.legend()
+
+ ax[1].plot(rpm, amp, label='I [A]')
+ ax[1].plot(rpm, vSid, label='V PCB [V]')
+ ax[1].plot(rpm, vbat, label='V Bat [V]')
+ ax[1].plot(rpm, thrust/((data[:, 9] / 4)), label='Efficency [g]/Watt')
+ ax[1].set_xlabel('RPM')
+ ax[1].set_ylabel('Voltage and Current')
+ ax[1].legend()
+ ax[1].grid(True)
+
+ ax[2].plot(thrust/((data[:, 9] / 4)))
+ ax[2].set_xlabel('time')
+ ax[2].set_ylabel('Efficency [g/W]')
+ ax[2].legend()
+ ax[2].grid(True)
+
+ ax3 = ax[2].twinx()
+ ax3.plot(thrust*4, color='tab:red')
+ ax3.set_ylabel('thrust [g]', color='tab:red')
+ ax3.legend()
+
+ fig.tight_layout()
+ plt.show()
\ No newline at end of file
diff --git a/tools/system_id/plot_motor_delay.py b/tools/system_id/plot_motor_delay.py
new file mode 100644
index 0000000000..efade79794
--- /dev/null
+++ b/tools/system_id/plot_motor_delay.py
@@ -0,0 +1,63 @@
+import matplotlib.pyplot as plt
+import numpy as np
+import argparse
+
+parser = argparse.ArgumentParser()
+parser.add_argument("file", help="csv file")
+args = parser.parse_args()
+
+data = np.loadtxt(args.file, delimiter=',', skiprows=1)
+
+# default
+c00 = 11.093358483549203
+c10 = -39.08104165843915
+c01 = -9.525647087583181
+c20 = 20.573302305476638
+c11 = 38.42885066644033
+
+def pwm2force(pwm, v):
+ pwm = pwm / 65536
+ v = v / 4.2
+ return c00 + c10 * pwm + c01 * v + c20 * pwm * pwm + c11 * pwm * v
+
+
+# u dot = lambda (u_des - u)
+l = 16
+
+u_pred = [0]
+dts = np.diff(data[:,0]) / 1e3
+for i, dt in enumerate(dts):
+ u = u_pred[-1]
+ pwm_des = data[i,2]
+ v = data[i,3]
+ # convert pwm -> force
+ f_des = pwm2force(pwm_des, v) * 4
+
+ u_dot = l * (f_des - u)
+ u_pred.append(u + u_dot * dt)
+u_pred = np.array(u_pred)
+
+fig, ax1 = plt.subplots()
+
+ax1.plot((data[:,0] - data[0,0])/1e3, data[:,1], 'b', label="Load cell")
+ax2 = ax1.twinx()
+ax2.plot((data[:,0] - data[0,0])/1e3, data[:,2], 'g', label="PWM")
+
+# ax1.plot((data[:,0] - data[0,0])/1e3, u_pred, 'r', label="Prediction")
+
+# ax1.plot((data[:,0] - data[0,0])/1e3, data[:,1], 'b', label="Load cell")
+
+# # compute f desired from pwm
+# f_des = []
+# for i in range(0, data.shape[0]):
+# f_des.append(pwm2force(data[i,2], data[i,3]) * 4)
+# f_des = np.array(f_des)
+
+# ax1.plot((data[:,0] - data[0,0])/1e3, f_des, 'g', label="Desired")
+# ax1.plot((data[:,0] - data[0,0])/1e3, u_pred, 'r', label="Prediction")
+# ax1.plot((data[:, 0] - data[0, 0])/1e3, data[:, 2], 'r', label="PWM")
+
+
+ax1.legend()
+
+plt.show()
diff --git a/tools/system_id/system_id.py b/tools/system_id/system_id.py
new file mode 100644
index 0000000000..a7d7e3247e
--- /dev/null
+++ b/tools/system_id/system_id.py
@@ -0,0 +1,114 @@
+import numpy as np
+from sklearn.linear_model import LinearRegression
+import matplotlib.pyplot as plt
+
+def loadFiles(filenames):
+ if len(filenames) == 0:
+ return None
+
+ Data = np.empty((0,5))
+ for filename in filenames:
+ fileData = np.loadtxt(filename, delimiter=',', skiprows=1)
+ lastValidIdx = np.argwhere( (fileData[:,2] < 3.7) & (fileData[:,1] == 0))
+ if len(lastValidIdx) > 0:
+ lastValidIdx = lastValidIdx[0][0]
+ else:
+ lastValidIdx = -1
+ print(lastValidIdx)
+ fileData = fileData[0:lastValidIdx]
+ print(fileData)
+ Data = np.vstack((Data, fileData))
+
+ filteredData = Data[ (Data[:,1] >= 8000) & (Data[:,1] <= 0.96 * 65536) ]
+
+ X = filteredData[:, 1] / 65536 # PWM
+ Y = filteredData[:, 2] / 4.2 # Vol
+ Z = filteredData[:, 0] / 4 # Thrust (gram)
+ maxThrust = filteredData[:, 3] / 4
+
+ return X, Y, Z, maxThrust
+
+def system_id(filenames, validations=[]):
+ X, Y, Z, maxThrust = loadFiles(filenames)
+ if len(validations) > 0:
+ X_val, Y_val, Z_val, maxThrust_val = loadFiles(validations)
+
+ plt.scatter(X, Z, label="training data")
+ plt.xlabel("PWM")
+ plt.ylabel("Thrust")
+
+ if len(validations) > 0:
+ plt.scatter(X_val, Z_val, label="validation data")
+ plt.xlabel("PWM")
+ plt.ylabel("Thrust")
+ plt.legend()
+ plt.show()
+
+ Input = np.zeros((X.shape[0], 4))
+ Input[:, 0] = X
+ Input[:, 1] = Y
+ Input[:, 2] = X ** 2
+ Input[:, 3] = X * Y
+
+ if len(validations) > 0:
+ Input_val = np.zeros((X_val.shape[0], 4))
+ Input_val[:, 0] = X_val
+ Input_val[:, 1] = Y_val
+ Input_val[:, 2] = X_val ** 2
+ Input_val[:, 3] = X_val * Y_val
+
+ reg = LinearRegression().fit(Input, Z)
+ print("******************************")
+ print("C_00 = " + str(reg.intercept_))
+ print("C_10 = " + str(reg.coef_[0]))
+ print("C_01 = " + str(reg.coef_[1]))
+ print("C_20 = " + str(reg.coef_[2]))
+ print("C_11 = " + str(reg.coef_[3]))
+ print("score = " + str(reg.score(Input, Z)))
+ if len(validations) > 0:
+ Error = reg.predict(Input_val) - Z_val.reshape(X_val.shape[0])
+ print("validation score = " + str(reg.score(Input_val, Z_val)))
+ print("validation max error = " + str(np.max(np.abs(Error))) + " gram")
+ print("validation mean error = " + str(np.mean(np.abs(Error))) + " gram")
+
+ Input = np.zeros((X.shape[0], 4))
+ Input[:, 0] = Z
+ Input[:, 1] = Y
+ Input[:, 2] = Z ** 2
+ Input[:, 3] = Z * Y
+
+ if len(validations) > 0:
+ Input_val = np.zeros((X_val.shape[0], 4))
+ Input_val[:, 0] = Z_val
+ Input_val[:, 1] = Y_val
+ Input_val[:, 2] = Z_val ** 2
+ Input_val[:, 3] = Z_val * Y_val
+
+ reg = LinearRegression().fit(Input, X)
+ print("******************************")
+ print("d00: " + str(reg.intercept_))
+ print("d10: " + str(reg.coef_[0]))
+ print("d01: " + str(reg.coef_[1]))
+ print("d20: " + str(reg.coef_[2]))
+ print("d11: " + str(reg.coef_[3]))
+ print("score = " + str(reg.score(Input, X)))
+ if len(validations) > 0:
+ Error = reg.predict(Input_val) - X_val.reshape(X_val.shape[0])
+ print("validation score = " + str(reg.score(Input_val, X_val)))
+ print("validation max error = " + str(np.max(np.abs(Error))) + " pwm")
+ print("validation mean error = " + str(np.mean(np.abs(Error))) + " pwm")
+
+ Input = np.zeros((X.shape[0], 2))
+ Input[:, 0] = X
+ Input[:, 1] = Y
+
+ reg = LinearRegression().fit(Input, maxThrust)
+ print("******************************")
+ print("e00: " + str(reg.intercept_))
+ print("e10: " + str(reg.coef_[0]))
+ print("e01: " + str(reg.coef_[1]))
+ print("score = " + str(reg.score(Input, maxThrust)))
+
+
+system_id(["data_max_thrust_brushless01.csv", "data_max_thrust_brushless03.csv"],
+ validations=["data_max_thrust_brushless01.csv"])