Skip to content
C

Add files via upload

Jobit Joseph2832e9e15833
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/app.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/app.h
new file mode 100644
index 0000000..b379ea2
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/app.h
@@ -0,0 +1,42 @@
+/*
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * LPS node firmware.
+ *
+ * Copyright 2019, Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Foobar is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Foobar. If not, see <http://www.gnu.org/licenses/>.
+ */
+/* app.h: App layer API */
+#pragma once
+
+/**
+ * App Inintialization
+ *
+ * Called during firmware initialization. A default weak function is implemented
+ * that creates a task and calls appMain() when the system is started.
+ *
+ * Implementation can be overwriten.
+ */
+void appInit();
+
+/**
+ * app main function, called when the Crazyflie has started from within a task created
+ * by appInit().
+ */
+void appMain();
\ No newline at end of file
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/app_channel.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/app_channel.h
new file mode 100644
index 0000000..f9a54c0
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/app_channel.h
@@ -0,0 +1,96 @@
+/*
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * LPS node firmware.
+ *
+ * Copyright 2020, Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Foobar is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Foobar. If not, see <http://www.gnu.org/licenses/>.
+ */
+/* app_channel.h: App realtime communication channel with the ground */
+#pragma once
+
+#include <stddef.h>
+#include <stdbool.h>
+
+#include "crtp.h"
+
+#define APPCHANNEL_WAIT_FOREVER (-1)
+#define APPCHANNEL_MTU (31)
+
+/**
+ * Send an app-channel packet
+ *
+ * The maximum buffer size that can be sent is define in APPCHANNEL_MTU.
+ * If the length of the buffer is longer than that, the packet will be cropped
+ * to send only the APPCHANNEL_MTU first bytes.
+ *
+ * This function can block if there is no more space in the Crazyflie TX queue.
+ * This is very unlikely to happen when CRTP is connected but can happen when the
+ * connection is not active.
+ *
+ * @param data Pointer to the data buffer to be sent
+ * @param length Length of the data buffer to send
+ *
+ * \app_api
+ */
+void appchannelSendPacket(void* data, size_t length);
+
+/**
+ * Receive an app-channel packet
+ *
+ * If the data received is longer than max_length, the data will be silently cropped and only
+ * the fist "max_length" bytes of the packet will be copied in the buffer.
+ *
+ * The maximum length packet possible to be received is APPCHANNEL_MTU bytes long.
+ *
+ * @param buffer Data buffer where the packet content will be copied
+ * @param max_length Maximum length of the data to be received, ie. length of the data buffer
+ * @param timeout_ms Time to wait for a packet in millisecond. A value of 0 will make the
+ * function non blocking, only reporting a packet is one is already in the
+ * receive queue. A value of APPCHANNEL_WAIT_FOREVER make the funciton block
+ * infinitly until a packet is received.
+ * @return 0 if no packet has been received. The data length of the packet received.
+ */
+size_t appchannelReceivePacket(void* buffer, size_t max_length, int timeout_ms);
+
+/**
+ * Returns if an overflow has occured in the receive queue
+ *
+ * The app-channel received packets are put in a queue. It is expected that the app is
+ * regularly calling appchannelReceivePacket() to get the packets from the receive queue.
+ * If that is not the case, the queue can overflow and this function allows the app to know
+ * about it. The overflow flag is being reset by this call.
+ *
+ * @return true if an overflow has occured in the receive queue.
+ */
+bool appchannelHasOverflowOccured();
+
+
+// Function declared bellow are private to the Crazyflie firmware and
+// should not be called from an app
+
+/**
+ *
+ */
+void appchannelInit();
+
+/**
+ *
+ */
+void appchannelIncomingPacket(CRTPPacket *p);
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/attitude_controller.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/attitude_controller.h
new file mode 100644
index 0000000..fe25dc0
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/attitude_controller.h
@@ -0,0 +1,77 @@
+/**
+ * || ____ _ __ ______
+ * +------+ / __ )(_) /_/ ____/_________ _____ ___
+ * | 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/>.
+ *
+ * attitude_controller.h: PID-based attitude controller
+ */
+
+#ifndef ATTITUDE_CONTROLLER_H_
+#define ATTITUDE_CONTROLLER_H_
+
+#include <stdbool.h>
+#include "commander.h"
+
+
+void attitudeControllerInit(const float updateDt);
+bool attitudeControllerTest(void);
+
+/**
+ * Make the controller run an update of the attitude PID. The output is
+ * the desired rate which should be fed into a rate controller. The
+ * attitude controller can be run in a slower update rate then the rate
+ * controller.
+ */
+void attitudeControllerCorrectAttitudePID(
+ float eulerRollActual, float eulerPitchActual, float eulerYawActual,
+ float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired,
+ float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired);
+
+/**
+ * Make the controller run an update of the rate PID. The output is
+ * the actuator force.
+ */
+void attitudeControllerCorrectRatePID(
+ float rollRateActual, float pitchRateActual, float yawRateActual,
+ float rollRateDesired, float pitchRateDesired, float yawRateDesired);
+
+/**
+ * Reset controller roll attitude PID
+ */
+void attitudeControllerResetRollAttitudePID(void);
+
+/**
+ * Reset controller pitch attitude PID
+ */
+void attitudeControllerResetPitchAttitudePID(void);
+
+/**
+ * Reset controller roll, pitch and yaw PID's.
+ */
+void attitudeControllerResetAllPID(void);
+
+/**
+ * Get the actuator output.
+ */
+void attitudeControllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* yaw);
+
+
+#endif /* ATTITUDE_CONTROLLER_H_ */
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/collision_avoidance.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/collision_avoidance.h
new file mode 100644
index 0000000..6ad6bcb
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/collision_avoidance.h
@@ -0,0 +1,146 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2020 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/>.
+ *
+ * collision_avoidance.h - Collision avoidance for multiple Crazyflies.
+ * Uses the buffered Voronoi collision avoidance algorithm:
+ *
+ * Zhou, Dingjiang, et al. "Fast, on-line collision avoidance for dynamic
+ * vehicles using buffered Voronoi cells." IEEE Robotics and Automation
+ * Letters 2.2 (2017): 1047-1054.
+ *
+ * Original author: James A. Preiss, University of Southern California, 2020.
+ */
+#ifndef __COLLISION_AVOIDANCE_H__
+#define __COLLISION_AVOIDANCE_H__
+
+
+#include "math3d.h"
+#include "stabilizer_types.h"
+
+
+// Algorithm parameters. They can be changed online by the user,
+// but the algorithm will never mutate them.
+
+typedef struct collision_avoidance_params_s
+{
+ // The Crazyflie's boundary for collision checking is a tall ellipsoid.
+ // This accounts for the downwash effect: Due to the fast-moving stream of
+ // air produced by the rotors, the safe distance to pass underneath another
+ // rotorcraft is much further than the safe distance to pass to the side.
+ struct vec ellipsoidRadii;
+
+ // The minimal and maximal corners of the bounding box in which we fly.
+ // These are applied to the Crazyflie's center point only;
+ // the ellipsoid collision volume is ignored.
+ struct vec bboxMin;
+ struct vec bboxMax;
+
+ // The BVC algorithm will ensure that we stay within our Voronoi cell
+ // for this duration. Higher values lead to more conservative behavior.
+ float horizonSecs;
+
+ // Maximum speed (meters/second) we will command. This is currently enforced
+ // loosely with an infinity-norm box, so be conservative.
+ float maxSpeed;
+
+ // If the velocity ray from our position (in velocity mode) or the segment
+ // connecting our current position to goal position (in position mode)
+ // intersects a cell wall, and the intersection is within this distance,
+ // activate the "sidestep" heuristic and move to the right to avoid deadlock
+ // situations.
+ float sidestepThreshold;
+
+ // If peer localization measurements are older than this, ignore them.
+ // If negative, never ignore.
+ int maxPeerLocAgeMillis;
+
+ // Tolerance for projecting position vectors into our Voronoi cell.
+ // Units are roughly Euclidean distance in meters, but not exactly.
+ float voronoiProjectionTolerance;
+
+ // Max number of iterations for projecting into our Voronoi cell.
+ // Using Dykstra's algorithm.
+ int voronoiProjectionMaxIters;
+
+} collision_avoidance_params_t;
+
+
+// Mutable state of the algorithm.
+
+typedef struct collision_avoidance_state_s
+{
+ // In case our Voronoi cell becomes empty, due to e.g. a disturbance, we will
+ // attempt to stop and stay in place. To avoid self-compounding drift , we
+ // must keep track of a fixed position value rather than using the current
+ // state as a setpoint.
+ struct vec lastFeasibleSetPosition;
+
+} collision_avoidance_state_t;
+
+
+// Main computational routine. Mutates the setpoint such that the new setpoint
+// respects the buffered Voronoi cell constraint.
+//
+// To facilitate compiling and testing on a PC, we take neighbor positions via
+// array instead of having the implementation call peer_localization.h functions
+// directly. On the other hand, we wish to use the minimum possible amount of
+// memory. Therefore, we allow the input and workspace arrays to overlap. If
+// otherPositions == workspace, this function will still work correctly, but it
+// will overwrite the contents of otherPositions.
+//
+// Args:
+// params: Algorithm parameters.
+// collisionState: Algorithm mutable state.
+// nOthers: Number of other Crazyflies in array arguments.
+// otherPositons: [nOthers * 3] array of positions (meters).
+// workspace: Space of no less than 7 * (nOthers + 6) floats. Used for
+// temporary storage during computation. This can be the same address as
+// otherPositions - otherPositions is copied into workspace immediately.
+// setpoint: Setpoint from commander that will be mutated.
+// sensorData: Not currently used, but kept for API similarity with sitAw.
+// state: Current state estimate.
+//
+void collisionAvoidanceUpdateSetpointCore(
+ collision_avoidance_params_t const *params,
+ collision_avoidance_state_t *collisionState,
+ int nOthers,
+ float const *otherPositions,
+ float *workspace,
+ setpoint_t *setpoint, sensorData_t const *sensorData, state_t const *state);
+
+// For ease of use, in a firmware build we include this wrapper that handles
+// the interaction with peer_localization and gets all parameter values via the
+// param system.
+#ifdef CRAZYFLIE_FW
+
+void collisionAvoidanceInit(void);
+bool collisionAvoidanceTest(void);
+
+// Wrapper that uses the peer localization system to construct the input arrays
+// for collisionAvoidanceUpdateSetpointCore.
+void collisionAvoidanceUpdateSetpoint(
+ setpoint_t *setpoint, sensorData_t const *sensorData, state_t const *state, uint32_t tick);
+
+#endif // CRAZYFLIE_FW defined
+
+#endif //__COLLISION_AVOIDANCE_H__
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/comm.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/comm.h
new file mode 100644
index 0000000..2b1325b
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/comm.h
@@ -0,0 +1,33 @@
+/*
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ * comm.c - High level communication module
+ */
+
+#ifndef __COMM_H__
+#define __COMM_H__
+
+void commInit(void);
+bool commTest(void);
+
+#endif //__COMM_H__
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/commander.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/commander.h
new file mode 100644
index 0000000..a6a4de6
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/commander.h
@@ -0,0 +1,58 @@
+/**
+ * || ____ _ __ ______
+ * +------+ / __ )(_) /_/ ____/_________ _____ ___
+ * | 0xBC | / __ / / __/ / / ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /___ / / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\____//_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2012 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef COMMANDER_H_
+#define COMMANDER_H_
+#include <stdint.h>
+#include <stdbool.h>
+#include "config.h"
+#include "stabilizer_types.h"
+
+#define DEFAULT_YAW_MODE XMODE
+
+#define COMMANDER_WDT_TIMEOUT_STABILIZE M2T(500)
+#define COMMANDER_WDT_TIMEOUT_SHUTDOWN M2T(2000)
+
+#define COMMANDER_PRIORITY_DISABLE 0
+#define COMMANDER_PRIORITY_CRTP 1
+#define COMMANDER_PRIORITY_EXTRX 2
+
+void commanderInit(void);
+bool commanderTest(void);
+uint32_t commanderGetInactivityTime(void);
+
+void commanderSetSetpoint(setpoint_t *setpoint, int priority);
+int commanderGetActivePriority(void);
+
+/* Inform the commander that streaming setpoints are about to stop.
+ * Parameter controls the amount of time the last setpoint will remain valid.
+ * This gives the PC time to send the next command, e.g. with the high-level
+ * commander, before we enter timeout mode.
+ */
+void commanderNotifySetpointsStop(int remainValidMillisecs);
+
+void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state);
+
+#endif /* COMMANDER_H_ */
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/console.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/console.h
new file mode 100644
index 0000000..1035a50
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/console.h
@@ -0,0 +1,81 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ * console.h - Used to send console data to the client
+ */
+
+#ifndef CONSOLE_H_
+#define CONSOLE_H_
+
+#include <stdbool.h>
+#include "eprintf.h"
+
+/**
+ * Initialize the console
+ */
+void consoleInit(void);
+
+bool consoleTest(void);
+
+/**
+ * Put a character to the console buffer
+ *
+ * @param ch character that shall be printed
+ * @return The character casted to unsigned int or EOF in case of error
+ */
+int consolePutchar(int ch);
+
+/**
+ * Put a character to the console buffer
+ *
+ * @param ch character that shall be printed
+ * @return The character casted to unsigned int or EOF in case of error
+ *
+ * @note This version can be called by interrup. In such case the internal
+ * buffer is going to be used. If a task currently is printing or if the
+ * interrupts prints too much the data will be ignored.
+ */
+int consolePutcharFromISR(int ch);
+
+/**
+ * Put a null-terminated string on the console buffer
+ *
+ * @param str Null terminated string
+ * @return a nonnegative number on success, or EOF on error.
+ */
+int consolePuts(char *str);
+
+/**
+ * Flush the console buffer
+ */
+void consoleFlush(void);
+
+/**
+ * Macro implementing consolePrintf with eprintf
+ *
+ * @param FMT String format
+ * @patam ... Parameters to print
+ */
+#define consolePrintf(FMT, ...) eprintf(consolePutchar, FMT, ## __VA_ARGS__)
+
+#endif /*CONSOLE_H_*/
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/controller.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/controller.h
new file mode 100644
index 0000000..a5042c0
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/controller.h
@@ -0,0 +1,48 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2016 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/>.
+ *
+ * controller.h - Controller interface
+ */
+#ifndef __CONTROLLER_H__
+#define __CONTROLLER_H__
+
+#include "stabilizer_types.h"
+
+typedef enum {
+ ControllerTypeAny,
+ ControllerTypePID,
+ ControllerTypeMellinger,
+ ControllerTypeINDI,
+ ControllerType_COUNT,
+} ControllerType;
+
+void controllerInit(ControllerType controller);
+bool controllerTest(void);
+void controller(control_t *control, setpoint_t *setpoint,
+ const sensorData_t *sensors,
+ const state_t *state,
+ const uint32_t tick);
+ControllerType getControllerType(void);
+const char* controllerGetName();
+
+#endif //__CONTROLLER_H__
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/controller_indi.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/controller_indi.h
new file mode 100644
index 0000000..98f3dc3
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/controller_indi.h
@@ -0,0 +1,105 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2016 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/>.
+ *
+ * controller_indi.h - INDI Controller Interface
+ */
+#ifndef __CONTROLLER_INDI_H__
+#define __CONTROLLER_INDI_H__
+
+#include "stabilizer_types.h"
+#include "filter.h"
+#include "math3d.h"
+#include "log.h"
+#include "param.h"
+#include "position_controller.h"
+#include "attitude_controller.h"
+#include "position_controller_indi.h"
+
+#define ATTITUDE_UPDATE_DT (float)(1.0f/ATTITUDE_RATE)
+
+// these parameters are used in the filtering of the angular acceleration
+#define STABILIZATION_INDI_FILT_CUTOFF 8.0f
+
+// the yaw sometimes requires more filtering
+#define STABILIZATION_INDI_FILT_CUTOFF_R STABILIZATION_INDI_FILT_CUTOFF
+
+// these parameters are used in the filtering of the angular acceleration
+#define STABILIZATION_INDI_G1_P 0.0066146f
+#define STABILIZATION_INDI_G1_Q 0.0052125f
+#define STABILIZATION_INDI_G1_R -0.001497f
+#define STABILIZATION_INDI_G2_R 0.000043475f
+#define STABILIZATION_INDI_REF_ERR_P 24.0f
+#define STABILIZATION_INDI_REF_ERR_Q 24.0f
+#define STABILIZATION_INDI_REF_ERR_R 24.0f
+#define STABILIZATION_INDI_REF_RATE_P 14.0f
+#define STABILIZATION_INDI_REF_RATE_Q 14.0f
+#define STABILIZATION_INDI_REF_RATE_R 14.0f
+#define STABILIZATION_INDI_ACT_DYN_P 0.03149f
+#define STABILIZATION_INDI_ACT_DYN_Q 0.03149f
+#define STABILIZATION_INDI_ACT_DYN_R 0.03149f
+
+/**
+ * @brief angular rates
+ * @details Units: rad/s */
+struct FloatRates {
+ float p; ///< in rad/s
+ float q; ///< in rad/s
+ float r; ///< in rad/s
+};
+
+struct ReferenceSystem {
+ float err_p;
+ float err_q;
+ float err_r;
+ float rate_p;
+ float rate_q;
+ float rate_r;
+};
+
+struct IndiVariables {
+ float thrust;
+ struct FloatRates angular_accel_ref;
+ struct FloatRates du;
+ struct FloatRates u_in;
+ struct FloatRates u_act_dyn;
+ float rate_d[3];
+
+ Butterworth2LowPass u[3];
+ Butterworth2LowPass rate[3];
+ struct FloatRates g1;
+ float g2;
+
+ struct ReferenceSystem reference_acceleration;
+ struct FloatRates act_dyn;
+ float filt_cutoff;
+ float filt_cutoff_r;
+};
+
+void controllerINDIInit(void);
+bool controllerINDITest(void);
+void controllerINDI(control_t *control, setpoint_t *setpoint,
+ const sensorData_t *sensors,
+ const state_t *state,
+ const uint32_t tick);
+
+#endif //__CONTROLLER_INDI_H__
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/controller_mellinger.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/controller_mellinger.h
new file mode 100644
index 0000000..eb3c162
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/controller_mellinger.h
@@ -0,0 +1,38 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2016 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/>.
+ *
+ * controller_mellinger.h - Mellinger Controller Interface
+ */
+#ifndef __CONTROLLER_MELLINGER_H__
+#define __CONTROLLER_MELLINGER_H__
+
+#include "stabilizer_types.h"
+
+void controllerMellingerInit(void);
+bool controllerMellingerTest(void);
+void controllerMellinger(control_t *control, setpoint_t *setpoint,
+ const sensorData_t *sensors,
+ const state_t *state,
+ const uint32_t tick);
+
+#endif //__CONTROLLER_MELLINGER_H__
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/controller_pid.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/controller_pid.h
new file mode 100644
index 0000000..3828a1f
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/controller_pid.h
@@ -0,0 +1,38 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2016 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/>.
+ *
+ * controller_pid.h - PID Controller Interface
+ */
+#ifndef __CONTROLLER_PID_H__
+#define __CONTROLLER_PID_H__
+
+#include "stabilizer_types.h"
+
+void controllerPidInit(void);
+bool controllerPidTest(void);
+void controllerPid(control_t *control, setpoint_t *setpoint,
+ const sensorData_t *sensors,
+ const state_t *state,
+ const uint32_t tick);
+
+#endif //__CONTROLLER_PID_H__
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/crtp.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/crtp.h
new file mode 100644
index 0000000..f06d7fd
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/crtp.h
@@ -0,0 +1,190 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ * crtp.h - CrazyRealtimeTransferProtocol stack
+ */
+
+#ifndef CRTP_H_
+#define CRTP_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#define CRTP_MAX_DATA_SIZE 30
+
+#define CRTP_HEADER(port, channel) (((port & 0x0F) << 4) | (channel & 0x0F))
+
+#define CRTP_IS_NULL_PACKET(P) ((P.header&0xF3)==0xF3)
+
+typedef enum {
+ CRTP_PORT_CONSOLE = 0x00,
+ CRTP_PORT_PARAM = 0x02,
+ CRTP_PORT_SETPOINT = 0x03,
+ CRTP_PORT_MEM = 0x04,
+ CRTP_PORT_LOG = 0x05,
+ CRTP_PORT_LOCALIZATION = 0x06,
+ CRTP_PORT_SETPOINT_GENERIC = 0x07,
+ CRTP_PORT_SETPOINT_HL = 0x08,
+ CRTP_PORT_PLATFORM = 0x0D,
+ CRTP_PORT_LINK = 0x0F,
+} CRTPPort;
+
+typedef struct _CRTPPacket
+{
+ uint8_t size; //< Size of data
+ union {
+ struct {
+ union {
+ uint8_t header; //< Header selecting channel and port
+ struct {
+#ifndef CRTP_HEADER_COMPAT
+ uint8_t channel : 2; //< Selected channel within port
+ uint8_t reserved : 2;
+ uint8_t port : 4; //< Selected port
+#else
+ uint8_t channel : 2;
+ uint8_t port : 4;
+ uint8_t reserved : 2;
+#endif
+ };
+ };
+ uint8_t data[CRTP_MAX_DATA_SIZE]; //< Data
+ };
+ uint8_t raw[CRTP_MAX_DATA_SIZE+1]; //< The full packet "raw"
+ };
+} __attribute__((packed)) CRTPPacket;
+
+typedef void (*CrtpCallback)(CRTPPacket *);
+
+/**
+ * Initialize the CRTP stack
+ */
+void crtpInit(void);
+
+bool crtpTest(void);
+
+/**
+ * Initializes the queue and dispatch of an task.
+ *
+ * @param[in] taskId The id of the CRTP task
+ */
+void crtpInitTaskQueue(CRTPPort taskId);
+
+/**
+ * Register a callback to be called for a particular port.
+ *
+ * @param[in] port Crtp port for which the callback is set
+ * @param[in] cb Callback that will be called when a packet is received on
+ * 'port'.
+ *
+ * @note Only one callback can be registered per port! The last callback
+ * registered will be the one called
+ */
+void crtpRegisterPortCB(int port, CrtpCallback cb);
+
+/**
+ * Put a packet in the TX task
+ *
+ * If the TX stack is full, the oldest lowest priority packet is dropped
+ *
+ * @param[in] p CRTPPacket to send
+ */
+int crtpSendPacket(CRTPPacket *p);
+
+/**
+ * Put a packet in the TX task
+ *
+ * If the TX stack is full, the function block until one place is free (Good for console implementation)
+ */
+int crtpSendPacketBlock(CRTPPacket *p);
+
+/**
+ * Fetch a packet with a specidied task ID.
+ *
+ * @param[in] taskId The id of the CRTP task
+ * @param[out] p The CRTP Packet with infomation (unchanged if nothing to fetch)
+ *
+ * @returns status of fetch from queue
+ */
+int crtpReceivePacket(CRTPPort taskId, CRTPPacket *p);
+
+/**
+ * Fetch a packet with a specidied task ID. Wait some time befor giving up
+ *
+ * @param[in] taskId The id of the CRTP task
+ * @param[out] p The CRTP Packet with infomation (unchanged if nothing to fetch)
+ * @param[in] wait Wait time in milisecond
+ *
+ * @returns status of fetch from queue
+ */
+int crtpReceivePacketWait(CRTPPort taskId, CRTPPacket *p, int wait);
+
+/**
+ * Get the number of free tx packets in the queue
+ *
+ * @return Number of free packets
+ */
+int crtpGetFreeTxQueuePackets(void);
+
+/**
+ * Wait for a packet to arrive for the specified taskID
+ *
+ * @param[in] taskId The id of the CRTP task
+ * @paran[out] p The CRTP Packet with information
+ *
+ * @return status of fetch from queue
+ */
+int crtpReceivePacketBlock(CRTPPort taskId, CRTPPacket *p);
+
+/**
+ * Function pointer structure to be filled by the CRTP link to permits CRTP to
+ * use manu link
+ */
+struct crtpLinkOperations
+{
+ int (*setEnable)(bool enable);
+ int (*sendPacket)(CRTPPacket *pk);
+ int (*receivePacket)(CRTPPacket *pk);
+ bool (*isConnected)(void);
+ int (*reset)(void);
+};
+
+void crtpSetLink(struct crtpLinkOperations * lk);
+
+/**
+ * Check if the connection timeout has been reached, otherwise
+ * we will assume that we are connected.
+ *
+ * @return true if conencted, otherwise false
+ */
+bool crtpIsConnected(void);
+
+/**
+ * Reset the CRTP communication by flushing all the queues that
+ * contain packages.
+ *
+ * @return 0 for success
+ */
+int crtpReset(void);
+
+#endif /*CRTP_H_*/
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/crtp_commander.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/crtp_commander.h
new file mode 100644
index 0000000..22a9114
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/crtp_commander.h
@@ -0,0 +1,45 @@
+/**
+ * || ____ _ __ ______
+ * +------+ / __ )(_) /_/ ____/_________ _____ ___
+ * | 0xBC | / __ / / __/ / / ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /___ / / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\____//_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2012 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef CRTP_COMMANDER_H_
+#define CRTP_COMMANDER_H_
+
+#include <stdint.h>
+#include "stabilizer_types.h"
+#include "crtp.h"
+
+typedef enum {
+ STABILIZE_MODE = 0x00,
+ ALTHOLD_MODE = 0x01,
+ POSHOLD_MODE = 0x02,
+ POSSET_MODE = 0x03,
+}FlightMode;
+
+void crtpCommanderInit(void);
+void crtpCommanderRpytDecodeSetpoint(setpoint_t *setpoint, CRTPPacket *pk);
+void crtpCommanderGenericDecodeSetpoint(setpoint_t *setpoint, CRTPPacket *pk);
+void setCommandermode(FlightMode mode);
+
+#endif /* CRTP_COMMANDER_H_ */
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/crtp_commander_high_level.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/crtp_commander_high_level.h
new file mode 100644
index 0000000..b5769f2
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/crtp_commander_high_level.h
@@ -0,0 +1,232 @@
+/*
+ * ______
+ * / ____/________ _____ __ ________ ______ __________ ___
+ * / / / ___/ __ `/_ / / / / / ___/ | /| / / __ `/ ___/ __ `__ \
+ * / /___/ / / /_/ / / /_/ /_/ (__ )| |/ |/ / /_/ / / / / / / / /
+ * \____/_/ \__,_/ /___/\__, /____/ |__/|__/\__,_/_/ /_/ /_/ /_/
+ * /____/
+ *
+ * Crazyswarm advanced control firmware for Crazyflie
+ *
+
+The MIT License (MIT)
+
+Copyright (c) 2018 Wolfgang Hoenig and James Alan Preiss
+
+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.
+*/
+
+/*
+Header file for high-level commander that computes smooth setpoints based on high-level inputs.
+*/
+
+#ifndef CRTP_COMMANDER_HIGH_LEVEL_H_
+#define CRTP_COMMANDER_HIGH_LEVEL_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "math3d.h"
+
+#include "stabilizer_types.h"
+
+#define NUM_TRAJECTORY_DEFINITIONS 10
+
+typedef enum {
+ CRTP_CHL_TRAJECTORY_TYPE_POLY4D = 0, // struct poly4d, see pptraj.h
+ CRTP_CHL_TRAJECTORY_TYPE_POLY4D_COMPRESSED = 1, // see pptraj_compressed.h
+ // Future types might include versions without yaw
+} crtpCommanderTrajectoryType_t;
+
+/* Public functions */
+void crtpCommanderHighLevelInit(void);
+
+// Retrieves the current setpoint
+void crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *state);
+
+// When flying sequences of high-level commands, the high-level commander uses
+// its own history of commands to determine the initial conditions of the next
+// trajectory it plans. However, when switching from a low-level streaming
+// setpoint mode to high-level, any past command history is invalid because of
+// the intervening low-level commands. Therefore, we must tell the high-level
+// commander what initial conditions to use for trajectory planning.
+void crtpCommanderHighLevelTellState(const state_t *state);
+
+// True if we have landed or emergency-stopped.
+bool crtpCommanderHighLevelIsStopped();
+
+// Public API - can be used from an app
+
+/**
+ * @brief vertical takeoff from current x-y position to given height
+ *
+ * @param absoluteHeight_m absolute target height (m)
+ * @param duration_s time it should take until target height is reached (s)
+ * @return zero if the command succeeded, an error code otherwise
+ */
+int crtpCommanderHighLevelTakeoff(const float absoluteHeight_m, const float duration_s);
+
+/**
+ * @brief vertical takeoff from current x-y position to given absolute or relative
+ * height with given velocity
+ *
+ * @param height_m absolute or relative target height (m)
+ * @param velocity_m_s takeoff velocity (m/s)
+ * @param relative whether the height is relative to the current position
+ * @return zero if the command succeeded, an error code otherwise
+ */
+int crtpCommanderHighLevelTakeoffWithVelocity(const float height_m, const float velocity_m_s, bool relative);
+
+
+/**
+ * @brief vertical takeoff from current x-y position to given height
+ *
+ * @param absoluteHeight_m absolute target height (m)
+ * @param duration_s time it should take until target height is reached (s)
+ * @param yaw yaw (rad)
+ * @return zero if the command succeeded, an error code otherwise
+ */
+int crtpCommanderHighLevelTakeoffYaw(const float absoluteHeight_m, const float duration_s, const float yaw);
+
+/**
+ * @brief vertical land from current x-y position to given height
+ *
+ * @param absoluteHeight_m absolute target height (m)
+ * @param duration_s time it should take until target height is reached (s)
+ * @return zero if the command succeeded, an error code otherwise
+ */
+int crtpCommanderHighLevelLand(const float absoluteHeight_m, const float duration_s);
+
+/**
+ * @brief vertical land from current x-y position to given absolute or relative
+ * height with given velocity
+ *
+ * @param height_m absolute or relative target height (m)
+ * @param velocity_m_s landing velocity (m/s)
+ * @param relative whether the height is relative to the current position
+ * @return zero if the command succeeded, an error code otherwise
+ */
+int crtpCommanderHighLevelLandWithVelocity(const float height_m, const float velocity_m_s, bool relative);
+
+/**
+ * @brief vertical land from current x-y position to given height
+ *
+ * @param absoluteHeight_m absolute target height (m)
+ * @param duration_s time it should take until target height is reached (s)
+ * @param yaw yaw (rad)
+ * @return zero if the command succeeded, an error code otherwise
+ */
+int crtpCommanderHighLevelLandYaw(const float absoluteHeight_m, const float duration_s, const float yaw);
+
+/**
+ * @brief stops the current trajectory (turns off the motors)
+ *
+ * Send the trajectory planner to idle state, where it has no plan. Also used when
+ * switching from high-level to low-level commands, or for emergencies.
+ *
+ * @return zero if the command succeeded, an error code otherwise. The function
+ * should never fail, but we provide the error code nevertheless for sake of
+ * consistency with the other high-level commander functions.
+ */
+int crtpCommanderHighLevelStop();
+
+/**
+ * @brief Go to an absolute or relative position
+ *
+ * @param x x (m)
+ * @param y y (m)
+ * @param z z (m)
+ * @param yaw yaw (rad)
+ * @param duration_s time it should take to reach the position (s)
+ * @param relative true if x, y, z is relative to the current position
+ * @return zero if the command succeeded, an error code otherwise
+ */
+int crtpCommanderHighLevelGoTo(const float x, const float y, const float z, const float yaw, const float duration_s, const bool relative);
+
+/**
+ * @brief Returns whether the trajectory with the given ID is defined
+ *
+ * @param trajectoryId The id of the trajectory
+ */
+bool crtpCommanderHighLevelIsTrajectoryDefined(uint8_t trajectoryId);
+
+/**
+ * @brief starts executing a specified trajectory
+ *
+ * @param trajectoryId id of the trajectory (previously defined by define_trajectory)
+ * @param timeScale time factor; 1.0 = original speed;
+ * >1.0: slower;
+ * <1.0: faster
+ * @param relative set to True, if trajectory should be shifted to current setpoint
+ * @param reversed set to True, if trajectory should be executed in reverse
+ * @return zero if the command succeeded, an error code otherwise
+ */
+int crtpCommanderHighLevelStartTrajectory(const uint8_t trajectoryId, const float timeScale, const bool relative, const bool reversed);
+
+/**
+ * @brief Define a trajectory that has previously been uploaded to memory.
+ *
+ * @param trajectoryId The id of the trajectory
+ * @param type The type of trajectory that is stored in memory.
+ * @param offset offset in uploaded memory (bytes)
+ * @param nPieces Nr of pieces in the trajectory
+ * @return zero if the command succeeded, an error code otherwise
+ */
+int crtpCommanderHighLevelDefineTrajectory(const uint8_t trajectoryId, const crtpCommanderTrajectoryType_t type, const uint32_t offset, const uint8_t nPieces);
+
+/**
+ * @brief Get the size of the allocated trajectory memory
+ *
+ * @return uint32_t The size of the trajectory memory in bytes
+ */
+uint32_t crtpCommanderHighLevelTrajectoryMemSize();
+
+/**
+ * @brief Copy trajectory data to the trajectory memeory. After the copy crtpCommanderHighLevelDefineTrajectory()
+ * must be called before the trajectory can be used.
+ *
+ * @param offset offset in uploaded memory (bytes)
+ * @param length Length of the data (bytes) to copy to the trajectory memory
+ * @param data[in] pointer to the trajectory data source
+ *
+ * @return true If data was copied
+ * @return false If data is too large
+ */
+bool crtpCommanderHighLevelWriteTrajectory(const uint32_t offset, const uint32_t length, const uint8_t* data);
+
+/**
+ * @brief Copy data from the trajectory memory.
+ *
+ * @param offset Offset in the trajectory memory (bytes)
+ * @param length Length of the data to copy
+ * @param destination [out] Pointer to copy data to
+ * @return true If data was copied
+ * @return false If length is too large
+ */
+bool crtpCommanderHighLevelReadTrajectory(const uint32_t offset, const uint32_t length, uint8_t* destination);
+
+/**
+ * @brief Query if the current trajectory has finished
+ *
+ * @return true The trajectory has reached the end
+ * @return false The trejectory is still running
+ */
+bool crtpCommanderHighLevelIsTrajectoryFinished();
+
+#endif /* CRTP_COMMANDER_HIGH_LEVEL_H_ */
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/crtp_localization_service.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/crtp_localization_service.h
new file mode 100644
index 0000000..81d370f
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/crtp_localization_service.h
@@ -0,0 +1,75 @@
+/**
+ * || ____ _ __ ______
+ * +------+ / __ )(_) /_/ ____/_________ _____ ___
+ * | 0xBC | / __ / / __/ / / ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /___ / / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\____//_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2012 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef _CRTP_LOCALIZATION_SERVICE_H_
+#define _CRTP_LOCALIZATION_SERVICE_H_
+
+#include "stabilizer_types.h"
+//#include "pulse_processor.h"
+
+/**
+ * CRTP external position data struct
+ */
+struct CrtpExtPosition
+{
+ float x; // in m
+ float y; // in m
+ float z; // in m
+} __attribute__((packed));
+
+struct CrtpExtPose
+{
+ float x; // in m
+ float y; // in m
+ float z; // in m
+ float qx;
+ float qy;
+ float qz;
+ float qw;
+} __attribute__((packed));
+
+typedef enum
+{
+ RANGE_STREAM_FLOAT = 0,
+ RANGE_STREAM_FP16 = 1,
+ LPS_SHORT_LPP_PACKET = 2,
+ EMERGENCY_STOP = 3,
+ EMERGENCY_STOP_WATCHDOG = 4,
+ COMM_GNSS_NMEA = 6,
+ COMM_GNSS_PROPRIETARY = 7,
+ EXT_POSE = 8,
+ EXT_POSE_PACKED = 9,
+ LH_ANGLE_STREAM = 10,
+ LH_PERSIST_DATA = 11,
+} locsrv_t;
+
+// Set up the callback for the CRTP_PORT_LOCALIZATION
+void locSrvInit(void);
+
+// Send range in float. After 5 ranges it will send the packet.
+void locSrvSendRangeFloat(uint8_t id, float range);
+//void locSrvSendLighthouseAngle(int basestation, pulseProcessorResult_t* angles);
+
+#endif /* _CRTP_LOCALIZATION_SERVICE_H_ */
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/crtpservice.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/crtpservice.h
new file mode 100644
index 0000000..e6505ba
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/crtpservice.h
@@ -0,0 +1,40 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ * link.h - Used to send/receive link packats
+ */
+
+#ifndef __CRTPSERVICE_H__
+#define __CRTPSERVICE_H__
+
+#include <stdbool.h>
+
+/**
+ * Initialize the link task
+ */
+void crtpserviceInit(void);
+
+bool crtpserviceTest(void);
+
+#endif /* __CRTPSERVICE_H__ */
+
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/estimator.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/estimator.h
new file mode 100644
index 0000000..f6db7a4
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/estimator.h
@@ -0,0 +1,58 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2016 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/>.
+ *
+ * estimator.h - State estimator interface
+ */
+#ifndef __ESTIMATOR_H__
+#define __ESTIMATOR_H__
+
+#include "stabilizer_types.h"
+
+typedef enum {
+ anyEstimator = 0,
+ complementaryEstimator,
+ kalmanEstimator,
+ StateEstimatorTypeCount,
+} StateEstimatorType;
+
+bool registerRequiredEstimator(StateEstimatorType estimator);
+StateEstimatorType deckGetRequiredEstimator();
+void stateEstimatorInit(StateEstimatorType estimator);
+bool stateEstimatorTest(void);
+void stateEstimatorSwitchTo(StateEstimatorType estimator);
+void stateEstimator(state_t *state, sensorData_t *sensors, control_t *control, const uint32_t tick);
+StateEstimatorType getStateEstimator(void);
+const char* stateEstimatorGetName();
+
+// Support to incorporate additional sensors into the state estimate via the following functions:
+bool estimatorEnqueueTDOA(const tdoaMeasurement_t *uwb);
+bool estimatorEnqueuePosition(const positionMeasurement_t *pos);
+bool estimatorEnqueuePose(const poseMeasurement_t *pose);
+bool estimatorEnqueueDistance(const distanceMeasurement_t *dist);
+bool estimatorEnqueueTOF(const tofMeasurement_t *tof);
+bool estimatorEnqueueAbsoluteHeight(const heightMeasurement_t *height);
+bool estimatorEnqueueFlow(const flowMeasurement_t *flow);
+bool estimatorEnqueueYawError(const yawErrorMeasurement_t *error);
+//bool estimatorEnqueueSweepAngles(const sweepAngleMeasurement_t *angles);
+
+#endif //__ESTIMATOR_H__
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/estimator_complementary.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/estimator_complementary.h
new file mode 100644
index 0000000..ae16432
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/estimator_complementary.h
@@ -0,0 +1,37 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2016 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/>.
+ *
+ * estimator_complementary.h - Complementary estimator interfaced
+ */
+#ifndef __ESTIMATOR_COMPLEMENTARY_H__
+#define __ESTIMATOR_COMPLEMENTARY_H__
+
+#include "stabilizer_types.h"
+
+void estimatorComplementaryInit(void);
+bool estimatorComplementaryTest(void);
+void estimatorComplementary(state_t *state, sensorData_t *sensors, control_t *control, const uint32_t tick);
+
+bool estimatorComplementaryEnqueueTOF(const tofMeasurement_t *tof);
+
+#endif //__ESTIMATOR_COMPLEMENTARY_H__
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/estimator_kalman.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/estimator_kalman.h
new file mode 100644
index 0000000..c391004
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/estimator_kalman.h
@@ -0,0 +1,88 @@
+/**
+ * Authored by Michael Hamer (http://www.mikehamer.info), June 2016
+ * Thank you to Mark Mueller (www.mwm.im) for advice during implementation,
+ * and for derivation of the original filter in the below-cited paper.
+ *
+ * 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/>.
+ *
+ * ============================================================================
+ *
+ * The Kalman filter implemented in this file is based on the papers:
+ *
+ * "Fusing ultra-wideband range measurements with accelerometers and rate gyroscopes for quadrocopter state estimation"
+ * http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=7139421
+ *
+ * and
+ *
+ * "Covariance Correction Step for Kalman Filtering with an Attitude"
+ * http://arc.aiaa.org/doi/abs/10.2514/1.G000848
+ *
+ * Academic citation would be appreciated.
+ *
+ * BIBTEX ENTRIES:
+ @INPROCEEDINGS{MuellerHamerUWB2015,
+ author = {Mueller, Mark W and Hamer, Michael and D'Andrea, Raffaello},
+ title = {Fusing ultra-wideband range measurements with accelerometers and rate gyroscopes for quadrocopter state estimation},
+ booktitle = {2015 IEEE International Conference on Robotics and Automation (ICRA)},
+ year = {2015},
+ month = {May},
+ pages = {1730-1736},
+ doi = {10.1109/ICRA.2015.7139421},
+ ISSN = {1050-4729}}
+
+ @ARTICLE{MuellerCovariance2016,
+ author={Mueller, Mark W and Hehn, Markus and D'Andrea, Raffaello},
+ title={Covariance Correction Step for Kalman Filtering with an Attitude},
+ journal={Journal of Guidance, Control, and Dynamics},
+ pages={1--7},
+ year={2016},
+ publisher={American Institute of Aeronautics and Astronautics}}
+ *
+ * ============================================================================
+ */
+
+#ifndef __ESTIMATOR_KALMAN_H__
+#define __ESTIMATOR_KALMAN_H__
+
+#include <stdint.h>
+#include "stabilizer_types.h"
+
+void estimatorKalmanInit(void);
+bool estimatorKalmanTest(void);
+void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control, const uint32_t tick);
+
+
+void estimatorKalmanTaskInit();
+bool estimatorKalmanTaskTest();
+
+/**
+ * The filter supports the incorporation of additional sensors into the state estimate via the following functions:
+ */
+bool estimatorKalmanEnqueueTDOA(const tdoaMeasurement_t *uwb);
+bool estimatorKalmanEnqueuePosition(const positionMeasurement_t *pos);
+bool estimatorKalmanEnqueuePose(const poseMeasurement_t *pose);
+bool estimatorKalmanEnqueueDistance(const distanceMeasurement_t *dist);
+bool estimatorKalmanEnqueueTOF(const tofMeasurement_t *tof);
+bool estimatorKalmanEnqueueAbsoluteHeight(const heightMeasurement_t *height);
+bool estimatorKalmanEnqueueFlow(const flowMeasurement_t *flow);
+bool estimatorKalmanEnqueueYawError(const yawErrorMeasurement_t* error);
+//bool estimatorKalmanEnqueueSweepAngles(const sweepAngleMeasurement_t *angles);
+
+void estimatorKalmanGetEstimatedPos(point_t* pos);
+
+/**
+ * Copies 9 floats representing the current state rotation matrix
+ */
+void estimatorKalmanGetEstimatedRot(float * rotationMatrix);
+
+#endif // __ESTIMATOR_KALMAN_H__
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/extrx.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/extrx.h
new file mode 100644
index 0000000..4a3dc33
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/extrx.h
@@ -0,0 +1,31 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2013 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/>.
+ *
+ *
+ * extrx.h - Module to handle external receiver inputs
+ */
+
+#include <stdint.h>
+
+void extRxInit(void);
+
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/info.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/info.h
new file mode 100644
index 0000000..64e7546
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/info.h
@@ -0,0 +1,43 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ * info.h - Receive information requests and send them back to the client
+ */
+#ifndef __INFO_H__
+#define __INFO_H__
+
+#include "crtp.h"
+
+/**
+ * Initialize the information task
+ *
+ */
+void infoInit();
+
+/**
+ * Battery minimum voltage before sending an automatic warning message
+ */
+#define INFO_BAT_WARNING 3.3
+
+#endif //__INFO_H__
+
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/kalman_core.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/kalman_core.h
new file mode 100644
index 0000000..1cae6e2
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/kalman_core.h
@@ -0,0 +1,151 @@
+/**
+ * Authored by Michael Hamer (http://www.mikehamer.info), June 2016
+ * Thank you to Mark Mueller (www.mwm.im) for advice during implementation,
+ * and for derivation of the original filter in the below-cited paper.
+ *
+ * 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/>.
+ *
+ * ============================================================================
+ *
+ * The Kalman filter implemented in this file is based on the papers:
+ *
+ * "Fusing ultra-wideband range measurements with accelerometers and rate gyroscopes for quadrocopter state estimation"
+ * http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=7139421
+ *
+ * and
+ *
+ * "Covariance Correction Step for Kalman Filtering with an Attitude"
+ * http://arc.aiaa.org/doi/abs/10.2514/1.G000848
+ *
+ * Academic citation would be appreciated.
+ *
+ * BIBTEX ENTRIES:
+ @INPROCEEDINGS{MuellerHamerUWB2015,
+ author = {Mueller, Mark W and Hamer, Michael and D'Andrea, Raffaello},
+ title = {Fusing ultra-wideband range measurements with accelerometers and rate gyroscopes for quadrocopter state estimation},
+ booktitle = {2015 IEEE International Conference on Robotics and Automation (ICRA)},
+ year = {2015},
+ month = {May},
+ pages = {1730-1736},
+ doi = {10.1109/ICRA.2015.7139421},
+ ISSN = {1050-4729}}
+
+ @ARTICLE{MuellerCovariance2016,
+ author={Mueller, Mark W and Hehn, Markus and D'Andrea, Raffaello},
+ title={Covariance Correction Step for Kalman Filtering with an Attitude},
+ journal={Journal of Guidance, Control, and Dynamics},
+ pages={1--7},
+ year={2016},
+ publisher={American Institute of Aeronautics and Astronautics}}
+ *
+ * ============================================================================
+ */
+
+#ifndef __KALMAN_CORE_H__
+#define __KALMAN_CORE_H__
+
+#include "cf_math.h"
+#include "stabilizer_types.h"
+
+// Indexes to access the quad's state, stored as a column vector
+typedef enum
+{
+ KC_STATE_X, KC_STATE_Y, KC_STATE_Z, KC_STATE_PX, KC_STATE_PY, KC_STATE_PZ, KC_STATE_D0, KC_STATE_D1, KC_STATE_D2, KC_STATE_DIM
+} kalmanCoreStateIdx_t;
+
+
+// The data used by the kalman core implementation.
+typedef struct {
+ /**
+ * Quadrocopter State
+ *
+ * The internally-estimated state is:
+ * - X, Y, Z: the quad's position in the global frame
+ * - PX, PY, PZ: the quad's velocity in its body frame
+ * - D0, D1, D2: attitude error
+ *
+ * For more information, refer to the paper
+ */
+ float S[KC_STATE_DIM];
+
+ // The quad's attitude as a quaternion (w,x,y,z)
+ // We store as a quaternion to allow easy normalization (in comparison to a rotation matrix),
+ // while also being robust against singularities (in comparison to euler angles)
+ float q[4];
+
+ // The quad's attitude as a rotation matrix (used by the prediction, updated by the finalization)
+ float R[3][3];
+
+ // The covariance matrix
+ __attribute__((aligned(4))) float P[KC_STATE_DIM][KC_STATE_DIM];
+ xtensa_matrix_instance_f32 Pm;
+
+ // Indicates that the internal state is corrupt and should be reset
+ bool resetEstimation;
+
+ float baroReferenceHeight;
+} kalmanCoreData_t;
+
+
+void kalmanCoreInit(kalmanCoreData_t* this);
+
+/* - Measurement updates based on sensors */
+
+// Barometer
+void kalmanCoreUpdateWithBaro(kalmanCoreData_t* this, float baroAsl, bool quadIsFlying);
+
+// Absolute height measurement along the room Z
+void kalmanCoreUpdateWithAbsoluteHeight(kalmanCoreData_t* this, heightMeasurement_t* height);
+
+// Direct measurements of Crazyflie position
+void kalmanCoreUpdateWithPosition(kalmanCoreData_t* this, positionMeasurement_t *xyz);
+
+// Direct measurements of Crazyflie pose
+void kalmanCoreUpdateWithPose(kalmanCoreData_t* this, poseMeasurement_t *pose);
+
+// Distance-to-point measurements
+void kalmanCoreUpdateWithDistance(kalmanCoreData_t* this, distanceMeasurement_t *d);
+
+// Measurements of a UWB Tx/Rx
+void kalmanCoreUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *tdoa);
+
+// Measurements of flow (dnx, dny)
+void kalmanCoreUpdateWithFlow(kalmanCoreData_t* this, const flowMeasurement_t *flow, const Axis3f *gyro);
+
+// Measurements of TOF from laser sensor
+void kalmanCoreUpdateWithTof(kalmanCoreData_t* this, tofMeasurement_t *tof);
+
+// Measurement of yaw error (outside measurement Vs current estimation)
+void kalmanCoreUpdateWithYawError(kalmanCoreData_t *this, yawErrorMeasurement_t *error);
+
+// Measurement of sweep angles from a Lighthouse base station
+//void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *angles, const uint32_t tick);
+
+/**
+ * Primary Kalman filter functions
+ *
+ * The filter progresses as:
+ * - Predicting the current state forward */
+void kalmanCorePredict(kalmanCoreData_t* this, float thrust, Axis3f *acc, Axis3f *gyro, float dt, bool quadIsFlying);
+
+void kalmanCoreAddProcessNoise(kalmanCoreData_t* this, float dt);
+
+/* - Finalization to incorporate attitude error into body attitude */
+void kalmanCoreFinalize(kalmanCoreData_t* this, uint32_t tick);
+
+/* - Externalization to move the filter's internal state into the external state expected by other modules */
+void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, const Axis3f *acc, uint32_t tick);
+
+void kalmanCoreDecoupleXY(kalmanCoreData_t* this);
+
+#endif // __KALMAN_CORE_H__
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/kalman_supervisor.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/kalman_supervisor.h
new file mode 100644
index 0000000..72191c9
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/kalman_supervisor.h
@@ -0,0 +1,32 @@
+/**
+ * ,---------, ____ _ __
+ * | ,-^-, | / __ )(_) /_______________ _____ ___
+ * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2019 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ * kalman_supervisor.h - Supervises the kalman estimator and makes sure it
+ * stays within bounds.
+ */
+
+#include <stdbool.h>
+#include "kalman_core.h"
+
+bool kalmanSupervisorIsStateWithinBounds(const kalmanCoreData_t* this);
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/lighthouse/lighthouse_core.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/lighthouse/lighthouse_core.h
new file mode 100644
index 0000000..c3ffac6
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/lighthouse/lighthouse_core.h
@@ -0,0 +1,64 @@
+/**
+ * ,---------, ____ _ __
+ * | ,-^-, | / __ )(_) /_______________ _____ ___
+ * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2019 - 2020 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/>.
+ *
+ *
+ * lighthouse_core.h - central part of the lighthouse positioning system
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "pulse_processor.h"
+
+typedef struct {
+ bool isSyncFrame;
+ pulseProcessorFrame_t data;
+} lighthouseUartFrame_t;
+
+typedef struct {
+ int sampleCount;
+ int hitCount;
+} lighthouseBsIdentificationData_t;
+
+void lighthouseCoreInit();
+void lighthouseCoreTask(void *param);
+
+/**
+ * @brief Set calibration data for one base station of the system
+ *
+ * @param baseStation The id of the base station
+ * @param calibration Pointer to calibration data
+ */
+void lighthouseCoreSetCalibrationData(const uint8_t baseStation, const lighthouseCalibration_t* calibration);
+
+/**
+ * @brief Copy current data in RAM to permanent storage
+ *
+ * @param baseStation The base station id to store data for
+ * @param geoData If true, write geometry data for the base station
+ * @param calibData if true, write calibration data for the base station
+ * @return true if data was stored
+ */
+bool lighthouseCorePersistData(const uint8_t baseStation, const bool geoData, const bool calibData);
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/lighthouse/lighthouse_deck_flasher.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/lighthouse/lighthouse_deck_flasher.h
new file mode 100644
index 0000000..529fcc7
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/lighthouse/lighthouse_deck_flasher.h
@@ -0,0 +1,31 @@
+/**
+ * ,---------, ____ _ __
+ * | ,-^-, | / __ )(_) /_______________ _____ ___
+ * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2020 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/>.
+ *
+ *
+ * lighthouse_deck_flasher.h - handles flashing of FPGA binaries on the
+ * Lighthouse deck
+ */
+
+#pragma once
+
+void lighthouseDeckFlasherCheckVersionAndBoot();
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/lighthouse/lighthouse_position_est.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/lighthouse/lighthouse_position_est.h
new file mode 100644
index 0000000..50d4c4b
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/lighthouse/lighthouse_position_est.h
@@ -0,0 +1,45 @@
+/**
+ * ,---------, ____ _ __
+ * | ,-^-, | / __ )(_) /_______________ _____ ___
+ * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2019 - 2020 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/>.
+ *
+ *
+ * lighthouse_position_est.h - position estimaton for the lighthouse system
+ */
+
+#pragma once
+
+#include "pulse_processor.h"
+#include "lighthouse_geometry.h"
+
+
+void lighthousePositionEstInit();
+
+/**
+ * @brief Set new geometry data for one base station
+ *
+ * @param baseStation The id of the base station
+ * @param geometry The geometry data
+ */
+void lighthousePositionSetGeometryData(const uint8_t baseStation, const baseStationGeometry_t* geometry);
+
+void lighthousePositionEstimatePoseCrossingBeams(const pulseProcessor_t *state, pulseProcessorResult_t* angles, int baseStation);
+void lighthousePositionEstimatePoseSweeps(const pulseProcessor_t *state, pulseProcessorResult_t* angles, int baseStation);
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/log.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/log.h
new file mode 100644
index 0000000..fa061f6
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/log.h
@@ -0,0 +1,196 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2012-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/>.
+ *
+ * log.h - Dynamic log system
+ */
+
+#ifndef __LOG_H__
+#define __LOG_H__
+
+#include <stdbool.h>
+#include <stdint.h>
+
+/* Public functions */
+void logInit(void);
+bool logTest(void);
+
+/* Public API to access of log variables */
+
+/** Variable identifier.
+ *
+ * Should be fetched with logGetVarId(). This is to be considered as an
+ * opaque type, internal structure might change.
+ *
+ * Use LOG_VARID_IS_VALID() to check if the ID is valid.
+ */
+typedef uint16_t logVarId_t;
+
+/** Get the varId from group and name of variable
+ *
+ * @param group Group name of the variable
+ * @param name Name of the variable
+ * @return The variable ID or an invalid ID. Use LOG_VARID_IS_VALID() to check validity.
+ */
+logVarId_t logGetVarId(char* group, char* name);
+
+/** Check variable ID validity
+ *
+ * @param varId variable ID, returned by logGetLogId()
+ * @return true if the variable ID is valid, false otherwise.
+ */
+#define LOG_VARID_IS_VALID(varId) (varId != 0xffffu)
+
+/** Return the logging type
+ *
+ * @param varId variable ID, returned by logGetVarId()
+ * @return Type of the variable. The value correspond to the defines used when
+ * declaring a param variable.
+ */
+int logGetType(logVarId_t varid);
+
+/** Get group and name strings of a parameter
+ *
+ * @param varId variable ID, returned by logGetVarId()
+ * @param group Pointer to a char* that will be filled with the group name
+ * @param group Pointer to a char* that will be filled with the variable name
+ *
+ * The string buffers must be able to hold at least 32 bytes.
+ */
+void logGetGroupAndName(logVarId_t varid, char** group, char** name);
+
+/** Get address of the logging variable
+ *
+ * @param varId variable ID, returned by logGetVarId()
+ * @return Address of the location of log variable
+ * */
+void* logGetAddress(logVarId_t varid);
+
+/** Get log variable size in byte
+ *
+ * @param type Type returned by logGetType()
+ * @return Size in byte occupied by variable of this type
+ */
+uint8_t logVarSize(int type);
+
+/** Return float value of a logging variable
+ *
+ * @param varId variable ID, returned by logGetVarId()
+ * @return Current value of the variable
+ */
+float logGetFloat(logVarId_t varid);
+
+/** Return int value of a logging variable
+ *
+ * @param varId variable ID, returned by logGetVarId()
+ * @return Current value of the variable
+ */
+int logGetInt(logVarId_t varid);
+
+/** Return Unsigned int value of a logging variable
+ *
+ * @param varId variable ID, returned by paramGetVarId()
+ * @return Current value of the variable
+ */
+unsigned int logGetUint(logVarId_t varid);
+
+/* Basic log structure */
+struct log_s {
+ uint8_t type;
+ char * name;
+ void * address;
+};
+
+/* Possible variable types */
+#define LOG_UINT8 1
+#define LOG_UINT16 2
+#define LOG_UINT32 3
+#define LOG_INT8 4
+#define LOG_INT16 5
+#define LOG_INT32 6
+#define LOG_FLOAT 7
+#define LOG_FP16 8
+
+typedef uint8_t (*logAcquireUInt8)(uint32_t timestamp, void* data);
+typedef uint16_t (*logAcquireUInt16)(uint32_t timestamp, void* data);
+typedef uint32_t (*logAcquireUInt32)(uint32_t timestamp, void* data);
+typedef int8_t (*logAcquireInt8)(uint32_t timestamp, void* data);
+typedef int16_t (*logAcquireInt16)(uint32_t timestamp, void* data);
+typedef int32_t (*logAcquireInt32)(uint32_t timestamp, void* data);
+typedef float (*logAcquireFloat)(uint32_t timestamp, void* data);
+
+typedef struct {
+ union {
+ logAcquireUInt8 acquireUInt8;
+ logAcquireUInt16 acquireUInt16;
+ logAcquireUInt32 acquireUInt32;
+ logAcquireInt8 acquireInt8;
+ logAcquireInt16 acquireInt16;
+ logAcquireInt32 acquireInt32;
+ logAcquireFloat aquireFloat;
+ };
+
+ void* data;
+} logByFunction_t;
+
+/* Internal defines */
+#define LOG_GROUP 0x80
+#define LOG_BY_FUNCTION 0x40
+#define LOG_START 1
+#define LOG_STOP 0
+
+/* Macros */
+
+#ifndef UNIT_TEST_MODE
+
+#define LOG_ADD(TYPE, NAME, ADDRESS) \
+ { .type = TYPE, .name = #NAME, .address = (void*)(ADDRESS), },
+
+#define LOG_ADD_BY_FUNCTION(TYPE, NAME, ADDRESS) \
+ { .type = TYPE | LOG_BY_FUNCTION, .name = #NAME, .address = (void*)(ADDRESS), },
+
+#define LOG_ADD_GROUP(TYPE, NAME, ADDRESS) \
+ { \
+ .type = TYPE, .name = #NAME, .address = (void*)(ADDRESS), },
+
+#define LOG_GROUP_START(NAME) \
+ static const struct log_s __logs_##NAME[] __attribute__((section(".log." #NAME), used)) = { \
+ LOG_ADD_GROUP(LOG_GROUP | LOG_START, NAME, 0x0)
+
+//#define LOG_GROUP_START_SYNC(NAME, LOCK) LOG_ADD_GROUP(LOG_GROUP | LOG_START, NAME, LOCK);
+
+#define LOG_GROUP_STOP(NAME) \
+ LOG_ADD_GROUP(LOG_GROUP | LOG_STOP, stop_##NAME, 0x0) \
+ };
+
+#else // UNIT_TEST_MODE
+
+// Empty defines when running unit tests
+#define LOG_ADD(TYPE, NAME, ADDRESS)
+#define LOG_ADD_BY_FUNCTION(TYPE, NAME, ADDRESS)
+#define LOG_ADD_GROUP(TYPE, NAME, ADDRESS)
+#define LOG_GROUP_START(NAME)
+#define LOG_GROUP_STOP(NAME)
+
+#endif // UNIT_TEST_MODE
+
+#endif /* __LOG_H__ */
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/math3d.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/math3d.h
new file mode 100644
index 0000000..673d5d6
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/math3d.h
@@ -0,0 +1,984 @@
+#pragma once
+
+/*
+The MIT License (MIT)
+
+cmath3d
+Copyright (c) 2016-2018 James Alan Preiss
+
+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.
+*/
+
+#include <math.h>
+#include <stdbool.h>
+#include <stddef.h>
+
+#ifdef CMATH3D_ASSERTS
+#include <assert.h>
+#endif
+
+#ifndef M_PI_F
+#define M_PI_F (3.14159265358979323846f)
+#define M_1_PI_F (0.31830988618379067154f)
+#define M_PI_2_F (1.57079632679f)
+#endif
+
+
+// ----------------------------- scalars --------------------------------
+
+static inline float fsqr(float x) { return x * x; }
+static inline float radians(float degrees) { return (M_PI_F / 180.0f) * degrees; }
+static inline float degrees(float radians) { return (180.0f / M_PI_F) * radians; }
+static inline float clamp(float value, float min, float max) {
+ if (value < min) return min;
+ if (value > max) return max;
+ return value;
+}
+// test two floats for approximate equality using the "consecutive floats have
+// consecutive bit representations" property. Argument `ulps` is the number of
+// steps to allow. this does not work well for numbers near zero.
+// See https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
+static inline bool fcloseulps(float a, float b, int ulps) {
+ if ((a < 0.0f) != (b < 0.0f)) {
+ // Handle negative zero.
+ if (a == b) {
+ return true;
+ }
+ return false;
+ }
+ int ia = *((int *)&a);
+ int ib = *((int *)&b);
+ return fabsf(ia - ib) <= ulps;
+}
+
+
+// ---------------------------- 3d vectors ------------------------------
+
+struct vec {
+ float x; float y; float z;
+};
+
+//
+// constructors
+//
+
+// construct a vector from 3 floats.
+static inline struct vec mkvec(float x, float y, float z) {
+ struct vec v;
+ v.x = x; v.y = y; v.z = z;
+ return v;
+}
+// construct a vector with the same value repeated for x, y, and z.
+static inline struct vec vrepeat(float x) {
+ return mkvec(x, x, x);
+}
+// construct a zero-vector.
+static inline struct vec vzero(void) {
+ return vrepeat(0.0f);
+}
+// construct the i'th basis vector, i.e. vbasis(0) == (1, 0, 0).
+static inline struct vec vbasis(int i) {
+ float a[3] = {0.0f, 0.0f, 0.0f};
+ a[i] = 1.0f;
+ return mkvec(a[0], a[1], a[2]);
+}
+
+//
+// operators
+//
+
+// multiply a vector by a scalar.
+static inline struct vec vscl(float s, struct vec v) {
+ return mkvec(s * v.x , s * v.y, s * v.z);
+}
+// negate a vector.
+static inline struct vec vneg(struct vec v) {
+ return mkvec(-v.x, -v.y, -v.z);
+}
+// divide a vector by a scalar.
+// does not perform divide-by-zero check.
+static inline struct vec vdiv(struct vec v, float s) {
+ return vscl(1.0f/s, v);
+}
+// add two vectors.
+static inline struct vec vadd(struct vec a, struct vec b) {
+ return mkvec(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+// subtract a vector from another vector.
+static inline struct vec vsub(struct vec a, struct vec b) {
+ return vadd(a, vneg(b));
+}
+// vector dot product.
+static inline float vdot(struct vec a, struct vec b) {
+ return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+// element-wise vector multiply.
+static inline struct vec veltmul(struct vec a, struct vec b) {
+ return mkvec(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+// element-wise vector divide.
+static inline struct vec veltdiv(struct vec a, struct vec b) {
+ return mkvec(a.x / b.x, a.y / b.y, a.z / b.z);
+}
+// element-wise vector reciprocal.
+static inline struct vec veltrecip(struct vec a) {
+ return mkvec(1.0f / a.x, 1.0f / a.y, 1.0f / a.z);
+}
+// vector magnitude squared.
+static inline float vmag2(struct vec v) {
+ return vdot(v, v);
+}
+// vector magnitude.
+static inline float vmag(struct vec v) {
+ return sqrtf(vmag2(v));
+}
+// vector Euclidean distance squared.
+static inline float vdist2(struct vec a, struct vec b) {
+ return vmag2(vsub(a, b));
+}
+// vector Euclidean distance.
+static inline float vdist(struct vec a, struct vec b) {
+ return sqrtf(vdist2(a, b));
+}
+// normalize a vector (make a unit vector).
+static inline struct vec vnormalize(struct vec v) {
+ return vdiv(v, vmag(v));
+}
+// clamp the Euclidean norm of a vector if it exceeds given maximum.
+static inline struct vec vclampnorm(struct vec v, float maxnorm) {
+ float const norm = vmag(v);
+ if (norm > maxnorm) {
+ return vscl(maxnorm / norm, v);
+ }
+ return v;
+}
+// vector cross product.
+static inline struct vec vcross(struct vec a, struct vec b) {
+ return mkvec(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x);
+}
+// projection of a onto b, where b is a unit vector.
+static inline struct vec vprojectunit(struct vec a, struct vec b_unit) {
+ return vscl(vdot(a, b_unit), b_unit);
+}
+// component of a orthogonal to b, where b is a unit vector.
+static inline struct vec vorthunit(struct vec a, struct vec b_unit) {
+ return vsub(a, vprojectunit(a, b_unit));
+}
+// element-wise absolute value of vector.
+static inline struct vec vabs(struct vec v) {
+ return mkvec(fabsf(v.x), fabsf(v.y), fabsf(v.z));
+}
+// element-wise minimum of vector.
+static inline struct vec vmin(struct vec a, struct vec b) {
+ return mkvec(fminf(a.x, b.x), fminf(a.y, b.y), fminf(a.z, b.z));
+}
+// element-wise maximum of vector.
+static inline struct vec vmax(struct vec a, struct vec b) {
+ return mkvec(fmaxf(a.x, b.x), fmaxf(a.y, b.y), fmaxf(a.z, b.z));
+}
+// element-wise clamp of vector.
+static inline struct vec vclamp(struct vec v, struct vec lower, struct vec upper) {
+ return vmin(upper, vmax(v, lower));
+}
+// element-wise clamp of vector to range centered on zero.
+static inline struct vec vclampabs(struct vec v, struct vec abs_upper) {
+ return vclamp(v, vneg(abs_upper), abs_upper);
+}
+// largest scalar element of vector.
+static inline float vmaxelt(struct vec v) {
+ return fmax(fmax(v.x, v.y), v.z);
+}
+// least (most negative) scalar element of vector.
+static inline float vminelt(struct vec v) {
+ return fmin(fmin(v.x, v.y), v.z);
+}
+// L1 norm (aka Minkowski, Taxicab, Manhattan norm) of a vector.
+static inline float vnorm1(struct vec v) {
+ return fabsf(v.x) + fabsf(v.y) + fabsf(v.z);
+}
+
+//
+// comparisons, including partial orderings
+//
+
+// compare two vectors for exact equality.
+static inline bool veq(struct vec a, struct vec b) {
+ return (a.x == b.x) && (a.y == b.y) && (a.z == b.z);
+}
+// compare two vectors for exact inequality.
+static inline bool vneq(struct vec a, struct vec b) {
+ return !veq(a, b);
+}
+// compare two vectors for near-equality with user-defined threshold.
+static inline bool veqepsilon(struct vec a, struct vec b, float epsilon) {
+ struct vec diffs = vabs(vsub(a, b));
+ return diffs.x < epsilon && diffs.y < epsilon && diffs.z < epsilon;
+}
+// all(element-wise less-than)
+static inline bool vless(struct vec a, struct vec b) {
+ return (a.x < b.x) && (a.y < b.y) && (a.z < b.z);
+}
+// all(element-wise less-than-or-equal)
+static inline bool vleq(struct vec a, struct vec b) {
+ return (a.x <= b.x) && (a.y <= b.y) && (a.z <= b.z);
+}
+// all(element-wise greater-than)
+static inline bool vgreater(struct vec a, struct vec b) {
+ return (a.x > b.x) && (a.y > b.y) && (a.z > b.z);
+}
+// all(element-wise greater-than-or-equal)
+static inline bool vgeq(struct vec a, struct vec b) {
+ return (a.x >= b.x) && (a.y >= b.y) && (a.z >= b.z);
+}
+// test if any element of a vector is NaN.
+static inline bool visnan(struct vec v) {
+ return isnan(v.x) || isnan(v.y) || isnan(v.z);
+}
+
+//
+// special functions to ease the pain of writing vector math in C.
+//
+
+// add 3 vectors.
+static inline struct vec vadd3(struct vec a, struct vec b, struct vec c) {
+ return vadd(vadd(a, b), c);
+}
+// add 4 vectors.
+static inline struct vec vadd4(struct vec a, struct vec b, struct vec c, struct vec d) {
+ // TODO: make sure it compiles to optimal code
+ return vadd(vadd(a, b), vadd(c, d));
+}
+// subtract b and c from a.
+static inline struct vec vsub2(struct vec a, struct vec b, struct vec c) {
+ return vadd3(a, vneg(b), vneg(c));
+}
+
+//
+// conversion to/from raw float and double arrays; array-like access.
+//
+
+// load a vector from a double array.
+static inline struct vec vload(double const *d) {
+ return mkvec(d[0], d[1], d[2]);
+}
+// store a vector into a double array.
+static inline void vstore(struct vec v, double *d) {
+ d[0] = (double)v.x; d[1] = (double)v.y; d[2] = (double)v.z;
+}
+// load a vector from a float array.
+static inline struct vec vloadf(float const *f) {
+ return mkvec(f[0], f[1], f[2]);
+}
+// store a vector into a float array.
+static inline void vstoref(struct vec v, float *f) {
+ f[0] = v.x; f[1] = v.y; f[2] = v.z;
+}
+// index a vector like a 3-element array.
+static inline float vindex(struct vec v, int i) {
+ return ((float const *)&v.x)[i];
+}
+
+
+// ---------------------------- 3x3 matrices ------------------------------
+
+struct mat33 {
+ float m[3][3];
+};
+
+//
+// constructors
+//
+
+// construct a zero matrix.
+static inline struct mat33 mzero(void) {
+ struct mat33 m;
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 3; ++j) {
+ m.m[i][j] = 0;
+ }
+ }
+ return m;
+}
+// construct a matrix with the given diagonal.
+static inline struct mat33 mdiag(float a, float b, float c) {
+ struct mat33 m = mzero();
+ m.m[0][0] = a;
+ m.m[1][1] = b;
+ m.m[2][2] = c;
+ return m;
+}
+// construct the matrix a * I for scalar a.
+static inline struct mat33 meyescl(float a) {
+ return mdiag(a, a, a);
+}
+// construct an identity matrix.
+static inline struct mat33 meye(void) {
+ return meyescl(1.0f);
+}
+// construct a matrix from three column vectors.
+static inline struct mat33 mcolumns(struct vec a, struct vec b, struct vec c) {
+ struct mat33 m;
+ m.m[0][0] = a.x;
+ m.m[1][0] = a.y;
+ m.m[2][0] = a.z;
+
+ m.m[0][1] = b.x;
+ m.m[1][1] = b.y;
+ m.m[2][1] = b.z;
+
+ m.m[0][2] = c.x;
+ m.m[1][2] = c.y;
+ m.m[2][2] = c.z;
+
+ return m;
+}
+// construct a matrix from three row vectors.
+static inline struct mat33 mrows(struct vec a, struct vec b, struct vec c) {
+ struct mat33 m;
+ vstoref(a, m.m[0]);
+ vstoref(b, m.m[1]);
+ vstoref(c, m.m[2]);
+ return m;
+}
+// construct the matrix A from vector v such that Ax = cross(v, x)
+static inline struct mat33 mcrossmat(struct vec v) {
+ struct mat33 m;
+ m.m[0][0] = 0;
+ m.m[0][1] = -v.z;
+ m.m[0][2] = v.y;
+ m.m[1][0] = v.z;
+ m.m[1][1] = 0;
+ m.m[1][2] = -v.x;
+ m.m[2][0] = -v.y;
+ m.m[2][1] = v.x;
+ m.m[2][2] = 0;
+ return m;
+}
+
+//
+// accessors
+//
+
+// return one column of a matrix as a vector.
+static inline struct vec mcolumn(struct mat33 m, int col) {
+ return mkvec(m.m[0][col], m.m[1][col], m.m[2][col]);
+}
+// return one row of a matrix as a vector.
+static inline struct vec mrow(struct mat33 m, int row) {
+ return vloadf(m.m[row]);
+}
+
+//
+// operators
+//
+
+// matrix transpose.
+static inline struct mat33 mtranspose(struct mat33 m) {
+ struct mat33 mt;
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 3; ++j) {
+ mt.m[i][j] = m.m[j][i];
+ }
+ }
+ return mt;
+}
+// multiply a matrix by a scalar.
+static inline struct mat33 mscl(float s, struct mat33 a) {
+ struct mat33 sa;
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 3; ++j) {
+ sa.m[i][j] = s * a.m[i][j];
+ }
+ }
+ return sa;
+}
+// negate a matrix.
+static inline struct mat33 mneg(struct mat33 a) {
+ return mscl(-1.0, a);
+}
+// add two matrices.
+static inline struct mat33 madd(struct mat33 a, struct mat33 b) {
+ struct mat33 c;
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 3; ++j) {
+ c.m[i][j] = a.m[i][j] + b.m[i][j];
+ }
+ }
+ return c;
+}
+// subtract two matrices.
+static inline struct mat33 msub(struct mat33 a, struct mat33 b) {
+ struct mat33 c;
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 3; ++j) {
+ c.m[i][j] = a.m[i][j] - b.m[i][j];
+ }
+ }
+ return c;
+}
+// multiply a matrix by a vector.
+static inline struct vec mvmul(struct mat33 a, struct vec v) {
+ float x = a.m[0][0] * v.x + a.m[0][1] * v.y + a.m[0][2] * v.z;
+ float y = a.m[1][0] * v.x + a.m[1][1] * v.y + a.m[1][2] * v.z;
+ float z = a.m[2][0] * v.x + a.m[2][1] * v.y + a.m[2][2] * v.z;
+ return mkvec(x, y, z);
+}
+// multiply two matrices.
+static inline struct mat33 mmul(struct mat33 a, struct mat33 b) {
+ struct mat33 ab;
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 3; ++j) {
+ float accum = 0;
+ for (int k = 0; k < 3; ++k) {
+ accum += a.m[i][k] * b.m[k][j];
+ }
+ ab.m[i][j] = accum;
+ }
+ }
+ return ab;
+}
+// add a scalar along the diagonal, i.e. a + dI.
+static inline struct mat33 maddridge(struct mat33 a, float d) {
+ a.m[0][0] += d;
+ a.m[1][1] += d;
+ a.m[2][2] += d;
+ return a;
+}
+// test if any element of a matrix is NaN.
+static inline bool misnan(struct mat33 m) {
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 3; ++j) {
+ if (isnan(m.m[i][j])) {
+ return true;
+ }
+ }
+ }
+ return false;
+}
+
+// set a 3x3 block within a big row-major matrix.
+// block: pointer to the upper-left element of the block in the big matrix.
+// stride: the number of columns in the big matrix.
+static inline void set_block33_rowmaj(float *block, int stride, struct mat33 const *m) {
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 3; ++j) {
+ block[j] = m->m[i][j];
+ }
+ block += stride;
+ }
+}
+
+//
+// special functions to ease the pain of writing vector math in C.
+//
+
+// add three matrices.
+static inline struct mat33 madd3(struct mat33 a, struct mat33 b, struct mat33 c) {
+ return madd(madd(a, b), c);
+}
+
+//
+// 3D rotation constructors & operators
+//
+
+// construct equivalent rotation matrix for axis-angle rotation.
+// assumes input axis is normalized, angle in radians.
+static inline struct mat33 maxisangle(struct vec axis, float angle) {
+ // Rodrigues formula
+ struct mat33 const K = mcrossmat(axis);
+ return madd3(
+ meye(),
+ mscl(sinf(angle), K),
+ mscl(1.0f - cosf(angle), mmul(K, K))
+ );
+}
+// rotation about x axis by given angle (radians)
+static inline struct mat33 mrotx(float angle) {
+ return maxisangle(mkvec(1.0f, 0.0f, 0.0f), angle);
+}
+// rotation about y axis by given angle (radians)
+static inline struct mat33 mroty(float angle) {
+ return maxisangle(mkvec(0.0f, 1.0f, 0.0f), angle);
+}
+// rotation about z axis by given angle (radians)
+static inline struct mat33 mrotz(float angle) {
+ return maxisangle(mkvec(0.0f, 0.0f, 1.0f), angle);
+}
+// TODO: these might be faster if written by hand,
+// but these are correct and the trig is probably the slow part anyway
+
+
+// Matrix TODO: inv, solve, eig, 9 floats ctor, axis-aligned rotations
+
+
+// ---------------------------- quaternions ------------------------------
+
+struct quat {
+ float x;
+ float y;
+ float z;
+ float w;
+};
+
+//
+// constructors
+//
+
+// construct a quaternion from its x, y, z, w elements.
+static inline struct quat mkquat(float x, float y, float z, float w) {
+ struct quat q;
+ q.x = x; q.y = y; q.z = z; q.w = w;
+ return q;
+}
+// construct a quaternion from a vector containing (x, y, z) and a scalar w.
+// note that this is NOT an axis-angle constructor.
+static inline struct quat quatvw(struct vec v, float w) {
+ struct quat q;
+ q.x = v.x; q.y = v.y; q.z = v.z;
+ q.w = w;
+ return q;
+}
+// construct an identity quaternion.
+static inline struct quat qeye(void) {
+ return mkquat(0, 0, 0, 1);
+}
+// construct a quaternion from an axis and angle of rotation.
+// does not assume axis is normalized.
+static inline struct quat qaxisangle(struct vec axis, float angle) {
+ float scale = sinf(angle / 2) / vmag(axis);
+ struct quat q;
+ q.x = scale * axis.x;
+ q.y = scale * axis.y;
+ q.z = scale * axis.z;
+ q.w = cosf(angle/2);
+ return q;
+}
+
+// fwd declare, needed in some ctors
+static inline struct quat qnormalize(struct quat q);
+
+// construct a quaternion such that q * a = b,
+// and the rotation axis is orthogonal to the plane defined by a and b,
+// and the rotation is less than 180 degrees.
+// assumes a and b are unit vectors.
+// does not handle degenerate case where a = -b. returns all-zero quat
+static inline struct quat qvectovec(struct vec a, struct vec b) {
+ struct vec const cross = vcross(a, b);
+ float const sinangle = vmag(cross);
+ float const cosangle = vdot(a, b);
+ // avoid taking sqrt of negative number due to floating point error.
+ // TODO: find tighter exact bound
+ float const EPS_ANGLE = 1e-6;
+ if (sinangle < EPS_ANGLE) {
+ if (cosangle > 0.0f) return qeye();
+ else return mkquat(0.0f, 0.0f, 0.0f, 0.0f); // degenerate
+ }
+ float const halfcos = 0.5f * cosangle;
+ // since angle is < 180deg, the positive sqrt is always correct
+ float const sinhalfangle = sqrtf(fmax(0.5f - halfcos, 0.0f));
+ float const coshalfangle = sqrtf(fmax(0.5f + halfcos, 0.0f));
+ struct vec const qimag = vscl(sinhalfangle / sinangle, cross);
+ float const qreal = coshalfangle;
+ return quatvw(qimag, qreal);
+}
+// construct from (roll, pitch, yaw) Euler angles using Tait-Bryan convention
+// (yaw, then pitch about new pitch axis, then roll about new roll axis)
+static inline struct quat rpy2quat(struct vec rpy) {
+ // from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+ float r = rpy.x;
+ float p = rpy.y;
+ float y = rpy.z;
+ float cr = cosf(r / 2.0f); float sr = sinf(r / 2.0f);
+ float cp = cosf(p / 2.0f); float sp = sinf(p / 2.0f);
+ float cy = cosf(y / 2.0f); float sy = sinf(y / 2.0f);
+
+ float qx = sr * cp * cy - cr * sp * sy;
+ float qy = cr * sp * cy + sr * cp * sy;
+ float qz = cr * cp * sy - sr * sp * cy;
+ float qw = cr * cp * cy + sr * sp * sy;
+
+ return mkquat(qx, qy, qz, qw);
+}
+// APPROXIMATE construction of a quaternion from small (roll, pitch, yaw) Euler angles
+// without computing any trig functions. only produces useful result for small angles.
+// Example application is integrating a gyroscope when the angular velocity
+// of the object is small compared to the sampling frequency.
+static inline struct quat rpy2quat_small(struct vec rpy) {
+ // TODO: cite source, but can be derived from rpy2quat under first-order approximation:
+ // sin(epsilon) = epsilon, cos(epsilon) = 1, epsilon^2 = 0
+ float q2 = vmag2(rpy) / 4.0f;
+ if (q2 < 1) {
+ return quatvw(vdiv(rpy, 2), sqrtf(1.0f - q2));
+ }
+ else {
+ float w = 1.0f / sqrtf(1.0f + q2);
+ return quatvw(vscl(w/2, rpy), w);
+ }
+}
+// construct quaternion from orthonormal matrix.
+static inline struct quat mat2quat(struct mat33 m) {
+ float w = sqrtf(fmax(0.0f, 1.0f + m.m[0][0] + m.m[1][1] + m.m[2][2])) / 2.0f;
+ float x = sqrtf(fmax(0.0f, 1.0f + m.m[0][0] - m.m[1][1] - m.m[2][2])) / 2.0f;
+ float y = sqrtf(fmax(0.0f, 1.0f - m.m[0][0] + m.m[1][1] - m.m[2][2])) / 2.0f;
+ float z = sqrtf(fmax(0.0f, 1.0f - m.m[0][0] - m.m[1][1] + m.m[2][2])) / 2.0f;
+ x = copysign(x, m.m[2][1] - m.m[1][2]);
+ y = copysign(y, m.m[0][2] - m.m[2][0]);
+ z = copysign(z, m.m[1][0] - m.m[0][1]);
+ return mkquat(x, y, z, w);
+}
+
+//
+// conversions to other parameterizations of 3D rotations
+//
+
+// convert quaternion to (roll, pitch, yaw) Euler angles using Tait-Bryan convention
+// (yaw, then pitch about new pitch axis, then roll about new roll axis)
+static inline struct vec quat2rpy(struct quat q) {
+ // from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+ struct vec v;
+ v.x = atan2f(2.0f * (q.w * q.x + q.y * q.z), 1 - 2 * (fsqr(q.x) + fsqr(q.y))); // roll
+ v.y = asinf(2.0f * (q.w * q.y - q.x * q.z)); // pitch
+ v.z = atan2f(2.0f * (q.w * q.z + q.x * q.y), 1 - 2 * (fsqr(q.y) + fsqr(q.z))); // yaw
+ return v;
+}
+// compute the axis of a quaternion's axis-angle decomposition.
+static inline struct vec quat2axis(struct quat q) {
+ // TODO this is not numerically stable for tiny rotations
+ float s = 1.0f / sqrtf(1.0f - q.w * q.w);
+ return vscl(s, mkvec(q.x, q.y, q.z));
+}
+// compute the angle of a quaternion's axis-angle decomposition.
+// result lies in domain (-pi, pi].
+static inline float quat2angle(struct quat q) {
+ float angle = 2 * acosf(q.w);
+ if (angle > M_PI_F) {
+ angle -= 2.0f * M_PI_F;
+ }
+ return angle;
+}
+// vector containing the imaginary part of the quaternion, i.e. (x, y, z)
+static inline struct vec quatimagpart(struct quat q) {
+ return mkvec(q.x, q.y, q.z);
+}
+// convert a quaternion into a 3x3 rotation matrix.
+static inline struct mat33 quat2rotmat(struct quat q) {
+ // from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+ float x = q.x;
+ float y = q.y;
+ float z = q.z;
+ float w = q.w;
+
+ struct mat33 m;
+ m.m[0][0] = 1 - 2*y*y - 2*z*z;
+ m.m[0][1] = 2*x*y - 2*z*w;
+ m.m[0][2] = 2*x*z + 2*y*w,
+ m.m[1][0] = 2*x*y + 2*z*w;
+ m.m[1][1] = 1 - 2*x*x - 2*z*z;
+ m.m[1][2] = 2*y*z - 2*x*w,
+ m.m[2][0] = 2*x*z - 2*y*w;
+ m.m[2][1] = 2*y*z + 2*x*w;
+ m.m[2][2] = 1 - 2*x*x - 2*y*y;
+ return m;
+}
+
+//
+// operators
+//
+
+// rotate a vector by a quaternion.
+static inline struct vec qvrot(struct quat q, struct vec v) {
+ // from http://gamedev.stackexchange.com/a/50545 - TODO find real citation
+ struct vec qv = mkvec(q.x, q.y, q.z);
+ return vadd3(
+ vscl(2.0f * vdot(qv, v), qv),
+ vscl(q.w * q.w - vmag2(qv), v),
+ vscl(2.0f * q.w, vcross(qv, v))
+ );
+}
+// multiply (compose) two quaternions
+// such that qvrot(qqmul(q, p), v) == qvrot(q, qvrot(p, v)).
+static inline struct quat qqmul(struct quat q, struct quat p) {
+ float x = q.w*p.x + q.z*p.y - q.y*p.z + q.x*p.w;
+ float y = -q.z*p.x + q.w*p.y + q.x*p.z + q.y*p.w;
+ float z = q.y*p.x - q.x*p.y + q.w*p.z + q.z*p.w;
+ float w = -q.x*p.x - q.y*p.y - q.z*p.z + q.w*p.w;
+ return mkquat(x, y, z, w);
+}
+// invert a quaternion.
+static inline struct quat qinv(struct quat q) {
+ return mkquat(-q.x, -q.y, -q.z, q.w);
+}
+// negate a quaternion.
+// this represents the same rotation, but is still sometimes useful.
+static inline struct quat qneg(struct quat q) {
+ return mkquat(-q.x, -q.y, -q.z, -q.w);
+}
+// return a quaternion representing the same rotation
+// but with a positive real term (q.w).
+// useful to collapse the double-covering of SO(3) by the quaternions.
+static inline struct quat qposreal(struct quat q) {
+ if (q.w < 0) return qneg(q);
+ return q;
+}
+// quaternion dot product. is cosine of angle between them.
+static inline float qdot(struct quat a, struct quat b) {
+ return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+static inline float qanglebetween(struct quat a, struct quat b) {
+ float const dot = qdot(qposreal(a), qposreal(b));
+ // prevent acos domain issues
+ if (dot > 1.0f - 1e9f) return 0.0f;
+ if (dot < -1.0f + 1e9f) return M_PI_F;
+ return acosf(dot);
+}
+static inline bool qeq(struct quat a, struct quat b) {
+ return a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w;
+}
+// normalize a quaternion.
+// typically used to mitigate precision errors.
+static inline struct quat qnormalize(struct quat q) {
+ float s = 1.0f / sqrtf(qdot(q, q));
+ return mkquat(s*q.x, s*q.y, s*q.z, s*q.w);
+}
+// update an attitude estimate quaternion with a reading from a gyroscope
+// over the timespan dt. Gyroscope is assumed (roll, pitch, yaw)
+// angular velocities in radians per second.
+static inline struct quat quat_gyro_update(struct quat quat, struct vec gyro, float const dt) {
+ // from "Indirect Kalman Filter for 3D Attitude Estimation", N. Trawny, 2005
+ struct quat q1;
+ float const r = (dt / 2) * gyro.x;
+ float const p = (dt / 2) * gyro.y;
+ float const y = (dt / 2) * gyro.z;
+
+ q1.x = quat.x + y*quat.y - p*quat.z + r*quat.w;
+ q1.y = -y*quat.x + quat.y + r*quat.z + p*quat.w;
+ q1.z = p*quat.x - r*quat.y + quat.z + y*quat.w;
+ q1.w = -r*quat.x - p*quat.y - y*quat.z + quat.w;
+ return q1;
+}
+// normalized linear interpolation. s should be between 0 and 1.
+static inline struct quat qnlerp(struct quat a, struct quat b, float t) {
+ float s = 1.0f - t;
+ return qnormalize(mkquat(
+ s*a.x + t*b.x, s*a.y + t*b.y, s*a.z + t*b.z, s*a.w + t*b.w));
+}
+// spherical linear interpolation. s should be between 0 and 1.
+static inline struct quat qslerp(struct quat a, struct quat b, float t)
+{
+ // from "Animating Rotation with Quaternion Curves", Ken Shoemake, 1985
+ float dp = qdot(a, b);
+ if (dp < 0) {
+ dp = -dp;
+ b = qneg(b);
+ }
+
+ if (dp > 0.99f) {
+ // fall back to linear interpolation to avoid div-by-zero
+ return qnlerp(a, b, t);
+ }
+ else {
+ float theta = acosf(dp);
+ float s = sinf(theta * (1 - t)) / sinf(theta);
+ t = sinf(theta * t) / sinf(theta);
+ return mkquat(
+ s*a.x + t*b.x, s*a.y + t*b.y, s*a.z + t*b.z, s*a.w + t*b.w);
+ }
+}
+
+//
+// conversion to/from raw float and double arrays.
+//
+
+// load a quaternion from a raw double array.
+static inline struct quat qload(double const *d) {
+ return mkquat(d[0], d[1], d[2], d[3]);
+}
+// store a quaternion into a raw double array.
+static inline void qstore(struct quat q, double *d) {
+ d[0] = (double)q.x; d[1] = (double)q.y; d[2] = (double)q.z; d[3] = (double)q.w;
+}
+// load a quaternion from a raw float array.
+static inline struct quat qloadf(float const *f) {
+ return mkquat(f[0], f[1], f[2], f[3]);
+}
+// store a quaternion into a raw float array.
+static inline void qstoref(struct quat q, float *f) {
+ f[0] = q.x; f[1] = q.y; f[2] = q.z; f[3] = q.w;
+}
+
+
+// ------------------------ convex sets in R^3 ---------------------------
+
+// project v onto the halfspace H = {x : a^T x <= b}, where a is a unit vector.
+// If v lies in H, returns v. Otherwise, returns the closest point, which
+// minimizes |x - v|_2, and will satisfy a^T x = b.
+static inline struct vec vprojecthalfspace(struct vec x, struct vec a_unit, float b) {
+ float ax = vdot(a_unit, x);
+ if (ax <= b) {
+ return x;
+ }
+ return vadd(x, vscl(b - ax, a_unit));
+}
+
+// test if v lies in the convex polytope defined by linear inequalities Ax <= b.
+// A: n x 3 matrix, row-major. Each row must have L2 norm of 1.
+// b: n vector.
+// tolerance: Allow violations up to Ax <= b + tolerance.
+static inline bool vinpolytope(struct vec v, float const A[], float const b[], int n, float tolerance)
+{
+ for (int i = 0; i < n; ++i) {
+ struct vec a = vloadf(A + 3 * i);
+ if (vdot(a, v) > b[i] + tolerance) {
+ return false;
+ }
+ }
+ return true;
+}
+
+// Finds the intersection between a ray and a convex polytope boundary.
+// The polytope is defined by the linear inequalities Ax <= b.
+// The ray must originate within the polytope.
+//
+// Args:
+// origin: Origin of the ray. Must lie within the polytope.
+// direction: Direction of the ray.
+// A: n x 3 matrix, row-major. Each row must have L2 norm of 1.
+// b: n vector.
+// n: Number of inequalities (rows in A).
+//
+// Returns:
+// s: positive float such that (origin + s * direction) is on the polytope
+// boundary. If the ray does not intersect the polytope -- for example, if
+// the polytope is unbounded -- then float INFINITY will be returned.
+// If the polytope is empty, then a negative number will be returned.
+// If `origin` does not lie within the polytope, return value is undefined.
+// active_row: output argument. The row in A (face of the polytope) that
+// the ray intersects. The point (origin + s * direction) will satisfy the
+// equation in that row with equality. If the ray intersects the polytope
+// at an intersection of two or more faces, active_row will be an arbitrary
+// member of the intersecting set. Optional, can be NULL.
+//
+static inline float rayintersectpolytope(struct vec origin, struct vec direction, float const A[], float const b[], int n, int *active_row)
+{
+ #ifdef CMATH3D_ASSERTS
+ // check for normalized input.
+ for (int i = 0; i < n; ++i) {
+ struct vec a = vloadf(A + 3 * i);
+ assert(fabsf(vmag2(a) - 1.0f) < 1e-6f);
+ }
+ #endif
+
+ float min_s = INFINITY;
+ int min_row = -1;
+
+ for (int i = 0; i < n; ++i) {
+ struct vec a = vloadf(A + 3 * i);
+ float a_dir = vdot(a, direction);
+ if (a_dir <= 0.0f) {
+ // The ray points away from or parallel to the polytope face,
+ // so it will never intersect.
+ continue;
+ }
+ // Solve for the intersection point of this halfspace algebraically.
+ float s = (b[i] - vdot(a, origin)) / a_dir;
+ if (s < min_s) {
+ min_s = s;
+ min_row = i;
+ }
+ }
+
+ if (active_row != NULL) {
+ *active_row = min_row;
+ }
+ return min_s;
+}
+
+// Projects v onto the convex polytope defined by linear inequalities Ax <= b.
+// Returns argmin_{x: Ax <= b} |x - v|_2. Uses Dykstra's (not Dijkstra's!)
+// projection algorithm [1] with robust stopping criteria [2].
+//
+// Args:
+// v: vector to project into the polytope.
+// A: n x 3 matrix, row-major. Each row must have L2 norm of 1.
+// b: n vector.
+// work: n x 3 matrix. will be overwritten. input values are not used.
+// tolerance: Stop when *approximately* violates the polytope constraints
+// by no more than this value. Not exact - be conservative if needed.
+// maxiters: Terminate after this many iterations regardless of convergence.
+//
+// Returns:
+// The projection of v into the polytope.
+//
+// References:
+// [1] Boyle, J. P., and Dykstra, R. L. (1986). A Method for Finding
+// Projections onto the Intersection of Convex Sets in Hilbert Spaces.
+// Lecture Notes in Statistics, 28–47. doi:10.1007/978-1-4613-9940-7_3
+// [2] Birgin, E. G., and Raydan, M. (2005). Robust Stopping Criteria for
+// Dykstra's Algorithm. SIAM J. Scientific Computing 26(4): 1405-1414.
+// doi:10.1137/03060062X
+//
+static inline struct vec vprojectpolytope(struct vec v, float const A[], float const b[], float work[], int n, float tolerance, int maxiters)
+{
+ // early bailout.
+ if (vinpolytope(v, A, b, n, tolerance)) {
+ return v;
+ }
+
+ #ifdef CMATH3D_ASSERTS
+ // check for normalized input.
+ for (int i = 0; i < n; ++i) {
+ struct vec a = vloadf(A + 3 * i);
+ assert(fabsf(vmag2(a) - 1.0f) < 1e-6f);
+ }
+ #endif
+
+ float *z = work;
+ for (int i = 0; i < 3 * n; ++i) {
+ z[i] = 0.0f;
+ }
+
+ // For user-friendliness, we accept a tolerance value in terms of
+ // the Euclidean magnitude of the polytope violation. However, the
+ // stopping criteria we wish to use is the robust one of [2] -
+ // specifically, the expression called c_I^k - which is based on the
+ // sum of squared projection residuals. This is a feeble attempt to get
+ // a ballpark tolerance value that is roughly equivalent.
+ float const tolerance2 = n * fsqr(tolerance) / 10.0f;
+ struct vec x = v;
+
+ for (int iter = 0; iter < maxiters; ++iter) {
+ float c = 0.0f;
+ for (int i = 0; i < n; ++i) {
+ struct vec x_old = x;
+ struct vec ai = vloadf(A + 3 * i);
+ struct vec zi_old = vloadf(z + 3 * i);
+ x = vprojecthalfspace(vsub(x_old, zi_old), ai, b[i]);
+ struct vec zi = vadd3(x, vneg(x_old), zi_old);
+ vstoref(zi, z + 3 * i);
+ c += vdist2(zi_old, zi);
+ }
+ if (c < tolerance2) {
+ return x;
+ }
+ }
+ return x;
+}
+
+
+// Overall TODO: lines? segments? planes? axis-aligned boxes? spheres?
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/mem.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/mem.h
new file mode 100644
index 0000000..913f499
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/mem.h
@@ -0,0 +1,67 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2012-2020 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/>.
+ *
+ * mem.h - Memory sub system
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+typedef enum {
+ MEM_TYPE_EEPROM = 0x00,
+ MEM_TYPE_OW = 0x01,
+ MEM_TYPE_LED12 = 0x10,
+ MEM_TYPE_LOCO = 0x11,
+ MEM_TYPE_TRAJ = 0x12,
+ MEM_TYPE_LOCO2 = 0x13,
+ MEM_TYPE_LH = 0x14,
+ MEM_TYPE_TESTER = 0x15,
+ MEM_TYPE_USD = 0x16,
+ MEM_TYPE_LEDMEM = 0x17,
+ MEM_TYPE_APP = 0x18,
+} MemoryType_t;
+
+#define MEMORY_SERIAL_LENGTH 8
+
+typedef struct {
+ const MemoryType_t type;
+ uint32_t (*getSize)(void);
+ bool (*read)(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer);
+ bool (*write)(const uint32_t memAddr, const uint8_t writeLen, const uint8_t* buffer);
+} MemoryHandlerDef_t;
+
+typedef struct {
+ uint8_t nrOfMems;
+ const uint32_t size;
+ bool (*getSerialNr)(const uint8_t selectedMem, uint8_t* serialNr);
+ bool (*read)(const uint8_t selectedMem, const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer);
+ bool (*write)(const uint8_t selectedMem, const uint32_t memAddr, const uint8_t writeLen, const uint8_t* buffer);
+} MemoryOwHandlerDef_t;
+
+/* Public functions */
+void memInit(void);
+bool memTest(void);
+void memoryRegisterHandler(const MemoryHandlerDef_t* handlerDef);
+void memoryRegisterOwHandler(const MemoryOwHandlerDef_t* handlerDef);
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/msp.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/msp.h
new file mode 100644
index 0000000..875899c
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/msp.h
@@ -0,0 +1,96 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ *
+ * msp.h - Definitions for the MultiWii Serial Protocol
+ *
+ * The MultiWii Serial Protocol is a commonly used protocol for OSDs,
+ * data logging, and communication with a ground telemetry station.
+ * For more details, see the official MSP documentation and support thread:
+ * - http://www.multiwii.com/wiki/index.php?title=Multiwii_Serial_Protocol
+ * - http://www.multiwii.com/forum/viewtopic.php?f=8&t=1516
+ */
+
+#ifndef MSP_H_
+#define MSP_H_
+#include <stdint.h>
+#include <stdbool.h>
+
+/**
+ * Function signature for a response callback to be provided by a client
+ */
+typedef void (*MspResponseCallback)(uint8_t* pBuffer, uint32_t bufferLen);
+
+/**
+ * Structure describing the MSP packet header
+ * This header is followed by a number of data bytes
+ * and a CRC. The CRC is the XOR of size, command, and all
+ * data bytes into a zeroed sum.
+ */
+typedef struct _MspHeader
+{
+ uint8_t preamble[2]; // Should always be '$M'
+ uint8_t direction;
+ uint8_t size;
+ uint8_t command;
+}__attribute__((packed)) MspHeader;
+
+/**
+ * Structure representing an instance of the MSP
+ * library. State is contained in this object, so
+ * there can be more than one MSP client in the system
+ */
+typedef struct
+{
+ // For storing MSP request data/state
+ MspHeader requestHeader;
+ uint8_t requestCrc;
+ uint8_t requestState;
+
+ // Response context
+ uint8_t mspResponse[256];
+ // Size of the complete response message contained
+ // in the mspResponse buffer
+ uint16_t mspResponseSize;
+
+ // The client callback to be invoked
+ // when a response message is ready
+ MspResponseCallback responseCallback;
+
+} MspObject;
+
+/**
+ * Initializes an instance of the MSP library
+ * @param[in] pMspObject Pointer to the MSP object to initialize
+ * @param[in] callback Function pointer to a callback
+ */
+void mspInit(MspObject* pMspObject, const MspResponseCallback callback);
+
+/**
+ * Processes the next byte received by the client
+ * @param[in] pMspObject Pointer to the MSB object
+ * @param[in] data The data to process
+ */
+void mspProcessByte(MspObject* pMspObject, const uint8_t data);
+
+#endif /* MSP_H_ */
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/outlierFilter.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/outlierFilter.h
new file mode 100644
index 0000000..ea6a81e
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/outlierFilter.h
@@ -0,0 +1,41 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Copyright (C) 2011-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/>.
+ *
+ * outlierFilter.h: Outlier rejection filter for the kalman filter
+ */
+
+#ifndef __OUTLIER_FILTER_H__
+#define __OUTLIER_FILTER_H__
+
+#include "stabilizer_types.h"
+
+bool outlierFilterValidateTdoaSimple(const tdoaMeasurement_t* tdoa);
+bool outlierFilterValidateTdoaSteps(const tdoaMeasurement_t* tdoa, const float error, const vector_t* jacobian, const point_t* estPos);
+
+typedef struct {
+ uint32_t openingTime;
+ int32_t openingWindow;
+} OutlierFilterLhState_t;
+bool outlierFilterValidateLighthouseSweep(OutlierFilterLhState_t* this, const float distanceToBs, const float angleError, const uint32_t now);
+void outlierFilterReset(OutlierFilterLhState_t* this, const uint32_t now);
+
+
+#endif // __OUTLIER_FILTER_H__
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/param.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/param.h
new file mode 100644
index 0000000..d8fb966
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/param.h
@@ -0,0 +1,196 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ * param.h - Crazy parameter system header file.
+ */
+
+#ifndef __PARAM_H__
+#define __PARAM_H__
+
+#include <stdbool.h>
+#include <stdint.h>
+
+/* Public functions */
+void paramInit(void);
+bool paramTest(void);
+
+/* Public API to access param variables */
+
+/** Variable identifier.
+ *
+ * Should be fetched with paramGetVarId(). This is to be considered as an
+ * opaque type, internal structure might change.
+ *
+ * Use PARAM_VARID_IS_VALID() to check if the ID is valid.
+ */
+typedef struct paramVarId_s {
+ uint16_t id;
+ uint16_t ptr;
+} __attribute__((packed)) paramVarId_t;
+
+/** Get the varId from group and name of variable
+ *
+ * @param group Group name of the variable
+ * @param name Name of the variable
+ * @return The variable ID or an invalid ID. Use PARAM_VARID_IS_VALID() to check validity.
+ */
+paramVarId_t paramGetVarId(char* group, char* name);
+
+/** Check variable ID validity
+ *
+ * @param varId variable ID, returned by paramGetVarId()
+ * @return true if the variable ID is valid, false otherwise.
+ */
+#define PARAM_VARID_IS_VALID(varId) (varId.id != 0xffffu)
+
+/** Return the parameter type
+ *
+ * @param varId variable ID, returned by paramGetVarId()
+ * @return Type of the variable. The value correspond to the defines used when
+ * declaring a param variable.
+ */
+int paramGetType(paramVarId_t varid);
+
+/** Get group and name strings of a parameter
+ *
+ * @param varId variable ID, returned by paramGetVarId()
+ * @param group Pointer to a char* that will be filled with the group name
+ * @param group Pointer to a char* that will be filled with the variable name
+ *
+ * The string buffers must be able to hold at least 32 bytes.
+ */
+void paramGetGroupAndName(paramVarId_t varid, char** group, char** name);
+
+/** Get parameter variable size in byte
+ *
+ * @param type Type returned by paramGetType()
+ * @return Size in byte occupied by variable of this type
+ */
+uint8_t paramVarSize(int type);
+
+/** Return float value of a parameter
+ *
+ * @param varId variable ID, returned by paramGetVarId()
+ * @return Current value of the variable
+ */
+float paramGetFloat(paramVarId_t varid);
+
+/** Return int value of a parameter
+ *
+ * @param varId variable ID, returned by paramGetVarId()
+ * @return Current value of the variable
+ */
+int paramGetInt(paramVarId_t varid);
+
+/** Return Unsigned int value of a paramter
+ *
+ * @param varId variable ID, returned by paramGetVarId()
+ * @return Current value of the variable
+ */
+unsigned int paramGetUint(paramVarId_t varid);
+
+/** Set int value of a parameter
+ *
+ * @param varId variable ID, returned by paramGetVarId()
+ * @param valuei Value to set in the variable
+ */
+void paramSetInt(paramVarId_t varid, int valuei);
+
+/** Set float value of a parameter
+ *
+ * @param varId variable ID, returned by paramGetVarId()
+ * @param valuef Value to set in the variable
+ */
+void paramSetFloat(paramVarId_t varid, float valuef);
+
+
+/* Basic parameter structure */
+struct param_s {
+ uint8_t type;
+ char * name;
+ void * address;
+};
+
+#define PARAM_BYTES_MASK 0x03
+#define PARAM_1BYTE 0x00
+#define PARAM_2BYTES 0x01
+#define PARAM_4BYTES 0x02
+#define PARAM_8BYTES 0x03
+
+#define PARAM_TYPE_INT (0x00<<2)
+#define PARAM_TYPE_FLOAT (0x01<<2)
+
+#define PARAM_SIGNED (0x00<<3)
+#define PARAM_UNSIGNED (0x01<<3)
+
+#define PARAM_VARIABLE (0x00<<7)
+#define PARAM_GROUP (0x01<<7)
+
+#define PARAM_RONLY (1<<6)
+
+#define PARAM_START 1
+#define PARAM_STOP 0
+
+#define PARAM_SYNC 0x02
+
+// User-friendly macros
+#define PARAM_UINT8 (PARAM_1BYTE | PARAM_TYPE_INT | PARAM_UNSIGNED)
+#define PARAM_INT8 (PARAM_1BYTE | PARAM_TYPE_INT | PARAM_SIGNED)
+#define PARAM_UINT16 (PARAM_2BYTES | PARAM_TYPE_INT | PARAM_UNSIGNED)
+#define PARAM_INT16 (PARAM_2BYTES | PARAM_TYPE_INT | PARAM_SIGNED)
+#define PARAM_UINT32 (PARAM_4BYTES | PARAM_TYPE_INT | PARAM_UNSIGNED)
+#define PARAM_INT32 (PARAM_4BYTES | PARAM_TYPE_INT | PARAM_SIGNED)
+
+#define PARAM_FLOAT (PARAM_4BYTES | PARAM_TYPE_FLOAT | PARAM_SIGNED)
+
+/* Macros */
+#ifndef UNIT_TEST_MODE
+
+#define PARAM_ADD(TYPE, NAME, ADDRESS) \
+ { .type = TYPE, .name = #NAME, .address = (void*)(ADDRESS), },
+
+#define PARAM_ADD_GROUP(TYPE, NAME, ADDRESS) \
+ { \
+ .type = TYPE, .name = #NAME, .address = (void*)(ADDRESS), },
+
+#define PARAM_GROUP_START(NAME) \
+ static const struct param_s __params_##NAME[] __attribute__((section(".param." #NAME), used)) = { \
+ PARAM_ADD_GROUP(PARAM_GROUP | PARAM_START, NAME, 0x0)
+
+//#define PARAM_GROUP_START_SYNC(NAME, LOCK) PARAM_ADD_GROUP(PARAM_GROUP | PARAM_START, NAME, LOCK);
+
+#define PARAM_GROUP_STOP(NAME) \
+ PARAM_ADD_GROUP(PARAM_GROUP | PARAM_STOP, stop_##NAME, 0x0) \
+ };
+
+#else // UNIT_TEST_MODE
+
+// Empty defines when running unit tests
+#define PARAM_ADD(TYPE, NAME, ADDRESS)
+#define PARAM_ADD_GROUP(TYPE, NAME, ADDRESS)
+#define PARAM_GROUP_START(NAME)
+#define PARAM_GROUP_STOP(NAME)
+
+#endif // UNIT_TEST_MODE
+
+#endif /* __PARAM_H__ */
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/peer_localization.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/peer_localization.h
new file mode 100644
index 0000000..3804f8e
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/peer_localization.h
@@ -0,0 +1,43 @@
+#ifndef __PEER_LOCALIZATION_H__
+#define __PEER_LOCALIZATION_H__
+
+#include <stdbool.h>
+#include "math3d.h"
+#include "stabilizer_types.h"
+
+// This module tracks the positions of other Crazyflies. Currently, only motion
+// capture localization is supported. Mocap setups transmit position
+// measurements on the radio in broadcast mode, so we can obtain the positions
+// of other Crazyflies on the same radio "for free". In the future, other
+// methods of peer localization such as peer-to-peer sharing could be added.
+
+// The maximum number of other Crazyflie ID's to track. This constant may be
+// needed for static allocations in other modules, e.g. collision avoidance.
+#define PEER_LOCALIZATION_MAX_NEIGHBORS 10
+
+// Initialize and test the module.
+void peerLocalizationInit();
+bool peerLocalizationTest();
+
+typedef struct peerLocalizationOtherPosition_s {
+ uint8_t id; // CF id
+ point_t pos; // position and timestamp (millisecs)
+} peerLocalizationOtherPosition_t;
+
+// Tell the peer localization system the position of another Crazyflie.
+// Should be called when the position is already known with high accuracy,
+// e.g. when a motion capture measurement packet is received.
+bool peerLocalizationTellPosition(int id, positionMeasurement_t const *pos);
+
+// Returns true if we have a position value for the given radio ID.
+bool peerLocalizationIsIDActive(uint8_t id);
+
+// Returns the position value for the given radio ID, or NULL if none exists.
+// Performs a linear search.
+peerLocalizationOtherPosition_t *peerLocalizationGetPositionByID(uint8_t id);
+
+// Returns the position value based on index, uncorrelated with radio ID. More
+// efficient if iterating over all peers is needed.
+peerLocalizationOtherPosition_t *peerLocalizationGetPositionByIdx(uint8_t idx);
+
+#endif // __PEER_LOCALIZATION_H__
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/pid.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/pid.h
new file mode 100644
index 0000000..f314318
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/pid.h
@@ -0,0 +1,227 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ *
+ * pid.h - implementation of the PID regulator
+ */
+#ifndef PID_H_
+#define PID_H_
+
+#include <stdbool.h>
+#include "filter.h"
+
+#ifdef CONFIG_TARGET_ESP32_S2_DRONE_V1_2
+ #define PID_ROLL_RATE_KP 190.0
+ #define PID_ROLL_RATE_KI 440.0
+ #define PID_ROLL_RATE_KD 2.6
+ #define PID_ROLL_RATE_INTEGRATION_LIMIT 33.3
+
+ #define PID_PITCH_RATE_KP 190.0
+ #define PID_PITCH_RATE_KI 440.0
+ #define PID_PITCH_RATE_KD 2.6
+ #define PID_PITCH_RATE_INTEGRATION_LIMIT 33.3
+
+ #define PID_YAW_RATE_KP 120.0
+ #define PID_YAW_RATE_KI 16.7
+ #define PID_YAW_RATE_KD 0.0
+ #define PID_YAW_RATE_INTEGRATION_LIMIT 166.7
+
+ #define PID_ROLL_KP 5.3
+ #define PID_ROLL_KI 2.5
+ #define PID_ROLL_KD 0.0
+ #define PID_ROLL_INTEGRATION_LIMIT 20.0
+
+ #define PID_PITCH_KP 5.3
+ #define PID_PITCH_KI 2.5
+ #define PID_PITCH_KD 0.0
+ #define PID_PITCH_INTEGRATION_LIMIT 20.0
+
+ #define PID_YAW_KP 6.0
+ #define PID_YAW_KI 1.0
+ #define PID_YAW_KD 0.35
+ #define PID_YAW_INTEGRATION_LIMIT 360.0
+
+
+ #define DEFAULT_PID_INTEGRATION_LIMIT 5000.0
+ #define DEFAULT_PID_OUTPUT_LIMIT 0.0
+#else
+ #define PID_ROLL_RATE_KP 250.0
+ #define PID_ROLL_RATE_KI 500.0
+ #define PID_ROLL_RATE_KD 2.5
+ #define PID_ROLL_RATE_INTEGRATION_LIMIT 33.3
+
+ #define PID_PITCH_RATE_KP 250.0
+ #define PID_PITCH_RATE_KI 500.0
+ #define PID_PITCH_RATE_KD 2.5
+ #define PID_PITCH_RATE_INTEGRATION_LIMIT 33.3
+
+ #define PID_YAW_RATE_KP 120.0
+ #define PID_YAW_RATE_KI 16.7
+ #define PID_YAW_RATE_KD 0.0
+ #define PID_YAW_RATE_INTEGRATION_LIMIT 166.7
+
+ #define PID_ROLL_KP 5.9
+ #define PID_ROLL_KI 2.9
+ #define PID_ROLL_KD 0.0
+ #define PID_ROLL_INTEGRATION_LIMIT 20.0
+
+ #define PID_PITCH_KP 5.9
+ #define PID_PITCH_KI 2.9
+ #define PID_PITCH_KD 0.0
+ #define PID_PITCH_INTEGRATION_LIMIT 20.0
+
+ #define PID_YAW_KP 6.0
+ #define PID_YAW_KI 1.0
+ #define PID_YAW_KD 0.35
+ #define PID_YAW_INTEGRATION_LIMIT 360.0
+
+
+ #define DEFAULT_PID_INTEGRATION_LIMIT 5000.0
+ #define DEFAULT_PID_OUTPUT_LIMIT 0.0
+#endif
+
+typedef struct
+{
+ float desired; //< set point
+ float error; //< error
+ float prevError; //< previous error
+ float integ; //< integral
+ float deriv; //< derivative
+ float kp; //< proportional gain
+ float ki; //< integral gain
+ float kd; //< derivative gain
+ float outP; //< proportional output (debugging)
+ float outI; //< integral output (debugging)
+ float outD; //< derivative output (debugging)
+ float iLimit; //< integral limit, absolute value. '0' means no limit.
+ float outputLimit; //< total PID output limit, absolute value. '0' means no limit.
+ float dt; //< delta-time dt
+ lpf2pData dFilter; //< filter for D term
+ bool enableDFilter; //< filter for D term enable flag
+} PidObject;
+
+/**
+ * PID object initialization.
+ *
+ * @param[out] pid A pointer to the pid object to initialize.
+ * @param[in] desired The initial set point.
+ * @param[in] kp The proportional gain
+ * @param[in] ki The integral gain
+ * @param[in] kd The derivative gain
+ * @param[in] dt Delta time since the last call
+ * @param[in] samplingRate Frequency the update will be called
+ * @param[in] cutoffFreq Frequency to set the low pass filter cutoff at
+ * @param[in] enableDFilter Enable setting for the D lowpass filter
+ */
+ void pidInit(PidObject* pid, const float desired, const float kp,
+ const float ki, const float kd, const float dt,
+ const float samplingRate, const float cutoffFreq,
+ bool enableDFilter);
+
+/**
+ * Set the integral limit for this PID in deg.
+ *
+ * @param[in] pid A pointer to the pid object.
+ * @param[in] limit Pid integral swing limit.
+ */
+void pidSetIntegralLimit(PidObject* pid, const float limit);
+
+/**
+ * Reset the PID error values
+ *
+ * @param[in] pid A pointer to the pid object.
+ * @param[in] limit Pid integral swing limit.
+ */
+void pidReset(PidObject* pid);
+
+/**
+ * Update the PID parameters.
+ *
+ * @param[in] pid A pointer to the pid object.
+ * @param[in] measured The measured value
+ * @param[in] updateError Set to TRUE if error should be calculated.
+ * Set to False if pidSetError() has been used.
+ * @return PID algorithm output
+ */
+float pidUpdate(PidObject* pid, const float measured, const bool updateError);
+
+/**
+ * Set a new set point for the PID to track.
+ *
+ * @param[in] pid A pointer to the pid object.
+ * @param[in] angle The new set point
+ */
+void pidSetDesired(PidObject* pid, const float desired);
+
+/**
+ * Set a new set point for the PID to track.
+ * @return The set point
+ */
+float pidGetDesired(PidObject* pid);
+
+/**
+ * Find out if PID is active
+ * @return TRUE if active, FALSE otherwise
+ */
+bool pidIsActive(PidObject* pid);
+
+/**
+ * Set a new error. Use if a special error calculation is needed.
+ *
+ * @param[in] pid A pointer to the pid object.
+ * @param[in] error The new error
+ */
+void pidSetError(PidObject* pid, const float error);
+
+/**
+ * Set a new proportional gain for the PID.
+ *
+ * @param[in] pid A pointer to the pid object.
+ * @param[in] kp The new proportional gain
+ */
+void pidSetKp(PidObject* pid, const float kp);
+
+/**
+ * Set a new integral gain for the PID.
+ *
+ * @param[in] pid A pointer to the pid object.
+ * @param[in] ki The new integral gain
+ */
+void pidSetKi(PidObject* pid, const float ki);
+
+/**
+ * Set a new derivative gain for the PID.
+ *
+ * @param[in] pid A pointer to the pid object.
+ * @param[in] kd The derivative gain
+ */
+void pidSetKd(PidObject* pid, const float kd);
+
+/**
+ * Set a new dt gain for the PID. Defaults to IMU_UPDATE_DT upon construction
+ *
+ * @param[in] pid A pointer to the pid object.
+ * @param[in] dt Delta time
+ */
+void pidSetDt(PidObject* pid, const float dt);
+#endif /* PID_H_ */
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/planner.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/planner.h
new file mode 100644
index 0000000..8fb5b09
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/planner.h
@@ -0,0 +1,108 @@
+/*
+ * ______
+ * / ____/________ _____ __ ________ ______ __________ ___
+ * / / / ___/ __ `/_ / / / / / ___/ | /| / / __ `/ ___/ __ `__ \
+ * / /___/ / / /_/ / / /_/ /_/ (__ )| |/ |/ / /_/ / / / / / / / /
+ * \____/_/ \__,_/ /___/\__, /____/ |__/|__/\__,_/_/ /_/ /_/ /_/
+ * /____/
+ *
+ * Crazyswarm advanced control firmware for Crazyflie
+ *
+
+The MIT License (MIT)
+
+Copyright (c) 2018 Wolfgang Hoenig and James Alan Preiss
+
+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.
+*/
+
+/*
+Header file for planning state machine
+*/
+
+#pragma once
+
+#include "math3d.h"
+#include "pptraj.h"
+#include "pptraj_compressed.h"
+
+enum trajectory_state
+{
+ TRAJECTORY_STATE_IDLE = 0,
+ TRAJECTORY_STATE_FLYING = 1,
+ TRAJECTORY_STATE_LANDING = 3,
+};
+
+enum trajectory_type
+{
+ TRAJECTORY_TYPE_PIECEWISE = 0,
+ TRAJECTORY_TYPE_PIECEWISE_COMPRESSED = 1
+};
+
+struct planner
+{
+ enum trajectory_state state; // current state
+ enum trajectory_type type; // current type
+ bool reversed; // true, if trajectory should be evaluated in reverse
+
+ union {
+ const struct piecewise_traj* trajectory; // pointer to trajectory
+ struct piecewise_traj_compressed* compressed_trajectory; // pointer to compressed trajectory
+ };
+
+ struct piecewise_traj planned_trajectory; // trajectory for on-board planning
+ struct poly4d pieces[1]; // the on-board planner requires a single piece, only
+};
+
+// initialize the planner
+void plan_init(struct planner *p);
+
+// tell the planner to stop.
+// subsequently, plan_is_stopped(p) will return true,
+// and it is no longer valid to call plan_current_goal(p).
+void plan_stop(struct planner *p);
+
+// query if the planner is stopped.
+// currently this is true at startup before we take off,
+// and also after an emergency stop.
+bool plan_is_stopped(struct planner *p);
+
+// get the planner's current goal.
+struct traj_eval plan_current_goal(struct planner *p, float t);
+
+// start a takeoff trajectory.
+int plan_takeoff(struct planner *p, struct vec curr_pos, float curr_yaw, float hover_height, float hover_yaw, float duration, float t);
+
+// start a landing trajectory.
+int plan_land(struct planner *p, struct vec curr_pos, float curr_yaw, float hover_height, float hover_yaw, float duration, float t);
+
+// move to a given position, then hover there.
+int plan_go_to(struct planner *p, bool relative, struct vec hover_pos, float hover_yaw, float duration, float t);
+
+// same as above, but with current state provided from outside.
+int plan_go_to_from(struct planner *p, const struct traj_eval *curr_eval, bool relative, struct vec hover_pos, float hover_yaw, float duration, float t);
+
+// start trajectory
+int plan_start_trajectory(struct planner *p, const struct piecewise_traj* trajectory, bool reversed);
+
+// start compressed trajectory
+int plan_start_compressed_trajectory(struct planner *p, struct piecewise_traj_compressed* trajectory);
+
+// Query if the trjectory is finished
+bool plan_is_finished(struct planner *p, float t);
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/platformservice.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/platformservice.h
new file mode 100644
index 0000000..d3992cd
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/platformservice.h
@@ -0,0 +1,44 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ * platformservice.h - Used to send platform commands
+ */
+
+#ifndef __PLATFORMSERVICE_H__
+#define __PLATFORMSERVICE_H__
+
+#include <stdbool.h>
+
+#include "crtp.h"
+
+/**
+ * Initialize the platform CRTP port
+ */
+void platformserviceInit(void);
+
+bool platformserviceTest(void);
+
+void platformserviceSendAppchannelPacket(CRTPPacket *p);
+
+#endif /* __PLATFORMSERVICE_H__ */
+
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/position_controller.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/position_controller.h
new file mode 100644
index 0000000..b6655f8
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/position_controller.h
@@ -0,0 +1,40 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2016 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ */
+#ifndef POSITION_CONTROLLER_H_
+#define POSITION_CONTROLLER_H_
+
+#include "stabilizer_types.h"
+
+// A position controller calculate the thrust, roll, pitch to approach
+// a 3D position setpoint
+void positionControllerInit();
+void positionControllerResetAllPID();
+void positionController(float* thrust, attitude_t *attitude, setpoint_t *setpoint,
+ const state_t *state);
+void velocityController(float* thrust, attitude_t *attitude, setpoint_t *setpoint,
+ const state_t *state);
+
+#endif /* POSITION_CONTROLLER_H_ */
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/position_controller_indi.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/position_controller_indi.h
new file mode 100644
index 0000000..2c7e375
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/position_controller_indi.h
@@ -0,0 +1,87 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2016 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/>.
+ *
+ * position_controller_indi.h - INDI Controller Interface
+ */
+
+#ifndef __POSITION_CONTROLLER_INDI_H__
+#define __POSITION_CONTROLLER_INDI_H__
+
+// Thrust command (in motor units)
+#define MIN_THRUST 0
+#define MAX_THRUST 60000
+
+// Cutoff frequency used in the filtering
+#define POSITION_INDI_FILT_CUTOFF 8.0f
+
+#include "controller_indi.h"
+#include "stabilizer_types.h"
+#include "filter.h"
+#include "math3d.h"
+#include "log.h"
+#include "param.h"
+
+struct Vectr {
+ float x;
+ float y;
+ float z;
+};
+
+struct Angles {
+ float phi;
+ float theta;
+ float psi;
+};
+
+struct IndiOuterVariables {
+
+ Butterworth2LowPass ddxi[3];
+ Butterworth2LowPass ang[3];
+ Butterworth2LowPass thr[3];
+
+ float filt_cutoff;
+ float act_dyn_posINDI;
+
+ struct Vectr linear_accel_ref;
+ struct Vectr linear_accel_err;
+ struct Vectr linear_accel_s; // acceleration sensed
+ struct Vectr linear_accel_f; // acceleration filtered
+ struct Vectr linear_accel_ft; // acceleration filtered transformed to NED
+ struct Angles attitude_s; // attitude senssed (here estimated)
+ struct Angles attitude_f; // attitude filtered
+ struct Angles attitude_c; // attitude commanded to the inner loop
+ float phi_tilde; // roll angle increment
+ float theta_tilde; // pitch angle increment
+ float T_tilde; // thrust increment
+ float T_inner; // thrust to inner INDI
+ float T_inner_f;
+ float T_incremented;
+};
+
+void positionControllerINDIInit(void);
+void positionControllerINDI(const sensorData_t *sensors,
+ setpoint_t *setpoint,
+ const state_t *state,
+ vector_t *refOuterINDI);
+
+#endif //__POSITION_CONTROLLER_INDI_H__
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/position_estimator.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/position_estimator.h
new file mode 100644
index 0000000..627092d
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/position_estimator.h
@@ -0,0 +1,34 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2016 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ */
+#ifndef POSITION_ESTIMATOR_H_
+#define POSITION_ESTIMATOR_H_
+
+#include "stabilizer_types.h"
+
+void positionEstimate(state_t* estimate, const sensorData_t* sensorData, const tofMeasurement_t* tofMeasurement, float dt, uint32_t tick);
+void positionUpdateVelocity(float accWZ, float dt);
+
+#endif /* POSITION_ESTIMATOR_H_ */
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/power_distribution.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/power_distribution.h
new file mode 100644
index 0000000..8167514
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/power_distribution.h
@@ -0,0 +1,37 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2016 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/>.
+ *
+ * power_distribution.h - Interface to stabilizer power distribution
+ */
+#ifndef __POWER_DISTRIBUTION_H__
+#define __POWER_DISTRIBUTION_H__
+
+#include "stabilizer_types.h"
+
+void powerDistributionInit(void);
+bool powerDistributionTest(void);
+void powerDistribution(const control_t *control);
+void powerStop();
+
+
+#endif //__POWER_DISTRIBUTION_H__
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/pptraj.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/pptraj.h
new file mode 100644
index 0000000..2dd48b5
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/pptraj.h
@@ -0,0 +1,181 @@
+/*
+ * ______
+ * / ____/________ _____ __ ________ ______ __________ ___
+ * / / / ___/ __ `/_ / / / / / ___/ | /| / / __ `/ ___/ __ `__ \
+ * / /___/ / / /_/ / / /_/ /_/ (__ )| |/ |/ / /_/ / / / / / / / /
+ * \____/_/ \__,_/ /___/\__, /____/ |__/|__/\__,_/_/ /_/ /_/ /_/
+ * /____/
+ *
+ * Crazyswarm advanced control firmware for Crazyflie
+ *
+
+The MIT License (MIT)
+
+Copyright (c) 2018 Wolfgang Hoenig and James Alan Preiss
+
+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.
+*/
+
+/*
+Header file for piecewise polynomial trajectories
+*/
+
+#pragma once
+
+#include "math3d.h"
+
+#define PP_DEGREE (7)
+#define PP_SIZE (PP_DEGREE + 1)
+
+
+//
+// 1d polynomial functions.
+// coefficients are stored in ascending order, i.e. p[0] is the constant term.
+//
+
+// evaluate a polynomial using horner's rule.
+float polyval(float const p[PP_SIZE], float t);
+
+// construct a linear polynomial from p(0) = x0 to p(duration) = x1.
+void polylinear(float p[PP_SIZE], float duration, float x0, float x1);
+
+// construct a polynomial from Bezier curve control points
+void polybezier(float p[PP_SIZE], float duration, float* x, int dim);
+
+// plan a degree-5 polynomial with the given duration T,
+// and given initial/final position, velocity, and acceleration
+void poly5(float poly[PP_SIZE], float T,
+ float x0, float dx0, float ddx0,
+ float xf, float dxf, float ddxf);
+
+// scale a polynomial in place.
+void polyscale(float p[PP_SIZE], float s);
+
+// compute the derivate of a polynomial in place.
+void polyder(float p[PP_SIZE]);
+
+// e.g. if s==2 the new polynomial will be stretched to take 2x longer
+void polystretchtime(float p[PP_SIZE], float s);
+
+// reflect a polynomial about the x-axis, e.g. p_after(-x) == p_before(x)
+void polyreflect(float p[PP_SIZE]);
+
+
+//
+// 4d single polynomial piece for x-y-z-yaw, includes duration.
+//
+
+struct poly4d
+{
+ float p[4][PP_SIZE];
+ float duration; // TODO use int millis instead?
+} __attribute__((packed));
+
+// construct a 4d zero polynomial.
+struct poly4d poly4d_zero(float duration);
+
+// construct a 4d linear polynomial.
+struct poly4d poly4d_linear(float duration,
+ struct vec p0, struct vec p1, float yaw0, float yaw1);
+
+// scale a 4d polynomial in-place.
+void poly4d_scale(struct poly4d *p, float x, float y, float z, float yaw);
+
+// shift a 4d polynomial by the given values in-place.
+void poly4d_shift(struct poly4d *p, float x, float y, float z, float yaw);
+static inline void poly4d_shift_vec(struct poly4d *p, struct vec pos, float yaw) {
+ poly4d_shift(p, pos.x, pos.y, pos.z, yaw);
+}
+
+// e.g. if s==2 the new polynomial will be stretched to take 2x longer.
+void poly4d_stretchtime(struct poly4d *p, float s);
+
+// compute the derivative of a 4d polynomial in-place.
+void polyder4d(struct poly4d *p);
+
+// compute loose maximum of acceleration -
+// uses L1 norm instead of Euclidean, evaluates polynomial instead of root-finding
+float poly4d_max_accel_approx(struct poly4d const *p);
+
+
+// output of differentially flat 4d polynomials.
+struct traj_eval
+{
+ struct vec pos;
+ struct vec vel;
+ struct vec acc;
+ struct vec omega;
+ float yaw;
+};
+
+// a traj_eval with all zero members.
+struct traj_eval traj_eval_zero(void);
+
+// a special value of traj_eval that indicates an invalid result.
+struct traj_eval traj_eval_invalid(void);
+
+// check if a traj_eval represents an invalid result.
+bool is_traj_eval_valid(struct traj_eval const *ev);
+
+// evaluate a single polynomial piece
+struct traj_eval poly4d_eval(struct poly4d const *p, float t);
+
+
+
+// ----------------------------------//
+// piecewise polynomial trajectories //
+// ----------------------------------//
+
+struct piecewise_traj
+{
+ float t_begin;
+ float timescale;
+ struct vec shift;
+ unsigned char n_pieces;
+ struct poly4d* pieces;
+};
+
+static inline float piecewise_duration(struct piecewise_traj const *pp)
+{
+ float total_dur = 0;
+ for (int i = 0; i < pp->n_pieces; ++i) {
+ total_dur += pp->pieces[i].duration;
+ }
+ return total_dur * pp->timescale;
+}
+
+void piecewise_plan_5th_order(struct piecewise_traj *p, float duration,
+ struct vec p0, float y0, struct vec v0, float dy0, struct vec a0,
+ struct vec p1, float y1, struct vec v1, float dy1, struct vec a1);
+
+void piecewise_plan_7th_order_no_jerk(struct piecewise_traj *p, float duration,
+ struct vec p0, float y0, struct vec v0, float dy0, struct vec a0,
+ struct vec p1, float y1, struct vec v1, float dy1, struct vec a1);
+
+struct traj_eval piecewise_eval(
+ struct piecewise_traj const *traj, float t);
+
+struct traj_eval piecewise_eval_reversed(
+ struct piecewise_traj const *traj, float t);
+
+
+static inline bool piecewise_is_finished(struct piecewise_traj const *traj, float t)
+{
+ return (t - traj->t_begin) >= piecewise_duration(traj);
+}
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/pptraj_compressed.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/pptraj_compressed.h
new file mode 100644
index 0000000..7707382
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/pptraj_compressed.h
@@ -0,0 +1,85 @@
+/*
+ * _____ ______ ___ __ ___ __ __ _
+ * / ___/__ / / / |/ /__ / /_ / _ \___ / / ___ / /_(_)______
+ * / /__/ _ \/ / / /|_/ / _ \/ __/ / , _/ _ \/ _ \/ _ \/ __/ / __(_-<
+ * \___/\___/_/_/_/ /_/\___/\__/ /_/|_|\___/_.__/\___/\__/_/\__/___/
+ *
+ * Compressed piecewise polynomial trajectories for Crazyflie.
+ *
+ * Copyright (c) 2019 CollMot Robotics. All rights reserved.
+ *
+ * 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/>.
+ *
+ * pptraj_compressed.h - Header file for compressed piecewise polynomial
+ * trajectories
+ */
+
+#pragma once
+
+#include "pptraj.h"
+#include <stdio.h>
+
+enum piecewise_traj_storage_type {
+ PPTRAJ_STORAGE_CONSTANT = 0,
+ PPTRAJ_STORAGE_LINEAR = 1,
+ PPTRAJ_STORAGE_BEZIER = 2,
+ PPTRAJ_STORAGE_FULL = 3
+};
+
+// ---------------------------------------------//
+// compressed piecewise polynomial trajectories //
+// ---------------------------------------------//
+
+struct piecewise_traj_compressed
+{
+ float t_begin;
+ float duration;
+ float timescale;
+ struct vec shift;
+ const void* data;
+
+ // mutable part of the data structure. We plan to mess around with this part
+ // but keep the rest untouched (i.e. supplied by the user)
+ struct {
+ // raw representation of the current piece
+ const void* data;
+
+ // start time of the current piece, relative to the "global" start time of
+ // the entire trajectory
+ float t_begin_relative;
+
+ // poly4d representation of the current piece
+ struct poly4d poly4d;
+ } current_piece;
+};
+
+// Returns the total duration of a compressed trajectory. The total duration
+// is pre-calculated and cached in the trajectory itself.
+static float piecewise_compressed_duration(struct piecewise_traj_compressed const *traj) {
+ return traj->duration;
+}
+
+// Returns whether we have finished flying the trajectory
+static inline bool piecewise_compressed_is_finished(
+ struct piecewise_traj_compressed const *traj, float t)
+{
+ return (t - traj->t_begin) >= piecewise_compressed_duration(traj);
+}
+
+// Evaluates the trajectory at the given time instant.
+struct traj_eval piecewise_compressed_eval(
+ struct piecewise_traj_compressed *traj, float t);
+
+// Loads the compressed trajectory at the given pointer
+void piecewise_compressed_load(
+ struct piecewise_traj_compressed *traj, const void* data);
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/quatcompress.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/quatcompress.h
new file mode 100644
index 0000000..6242343
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/quatcompress.h
@@ -0,0 +1,86 @@
+#ifndef QUATCOMPRESS_H
+#define QUATCOMPRESS_H
+
+/*
+
+MIT License
+
+Copyright (c) 2016 James A. Preiss
+
+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.
+
+*/
+
+#include <stdint.h>
+#include <math.h>
+
+// assumes input quaternion is normalized. will fail if not.
+static inline uint32_t quatcompress(float const q[4])
+{
+ // we send the values of the quaternion's smallest 3 elements.
+ unsigned i_largest = 0;
+ for (unsigned i = 1; i < 4; ++i) {
+ if (fabsf(q[i]) > fabsf(q[i_largest])) {
+ i_largest = i;
+ }
+ }
+
+ // since -q represents the same rotation as q,
+ // transform the quaternion so the largest element is positive.
+ // this avoids having to send its sign bit.
+ unsigned negate = q[i_largest] < 0;
+
+ // 1/sqrt(2) is the largest possible value
+ // of the second-largest element in a unit quaternion.
+
+ // do compression using sign bit and 9-bit precision per element.
+ uint32_t comp = i_largest;
+ for (unsigned i = 0; i < 4; ++i) {
+ if (i != i_largest) {
+ unsigned negbit = (q[i] < 0) ^ negate;
+ unsigned mag = ((1 << 9) - 1) * (fabsf(q[i]) / (float)M_SQRT1_2) + 0.5f;
+ comp = (comp << 10) | (negbit << 9) | mag;
+ }
+ }
+
+ return comp;
+}
+
+static inline void quatdecompress(uint32_t comp, float q[4])
+{
+ unsigned const mask = (1 << 9) - 1;
+
+ int const i_largest = comp >> 30;
+ float sum_squares = 0;
+ for (int i = 3; i >= 0; --i) {
+ if (i != i_largest) {
+ unsigned mag = comp & mask;
+ unsigned negbit = (comp >> 9) & 0x1;
+ comp = comp >> 10;
+ q[i] = ((float)M_SQRT1_2) * ((float)mag) / mask;
+ if (negbit == 1) {
+ q[i] = -q[i];
+ }
+ sum_squares += q[i] * q[i];
+ }
+ }
+ q[i_largest] = sqrtf(1.0f - sum_squares);
+}
+
+#endif // QUATCOMPRESS_H
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/queuemonitor.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/queuemonitor.h
new file mode 100644
index 0000000..5018e53
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/queuemonitor.h
@@ -0,0 +1,46 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ * log.h - Dynamic log system
+ */
+
+#ifndef __QUEUE_MONITOR_H__
+#define __QUEUE_MONITOR_H__
+
+
+#include "FreeRTOS.h"
+
+#ifdef DEBUG_QUEUE_MONITOR
+ #include "queue.h"
+
+ void queueMonitorInit();
+ #define DEBUG_QUEUE_MONITOR_REGISTER(queue) qmRegisterQueue(queue, __FILE__, #queue)
+
+ void qm_traceQUEUE_SEND(void* xQueue);
+ void qm_traceQUEUE_SEND_FAILED(void* xQueue);
+ void qmRegisterQueue(xQueueHandle* xQueue, char* fileName, char* queueName);
+#else
+ #define DEBUG_QUEUE_MONITOR_REGISTER(queue)
+#endif // DEBUG_QUEUE_MONITOR
+
+#endif // __QUEUE_MONITOR_H__
\ No newline at end of file
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/range.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/range.h
new file mode 100644
index 0000000..c690420
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/range.h
@@ -0,0 +1,64 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie Firmware
+ *
+ * Copyright (C) 2018 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * ranges.h: Centralize range measurements for different directions
+ * and make them available as log
+ */
+
+#pragma once
+
+typedef enum {
+ rangeFront=0,
+ rangeBack,
+ rangeLeft,
+ rangeRight,
+ rangeUp,
+ rangeDown,
+ RANGE_T_END,
+} rangeDirection_t;
+
+/**
+ * Set the range for a certain direction
+ *
+ * @param direction Direction of the range
+ * @param range_m Distance to an object in meter
+ */
+void rangeSet(rangeDirection_t direction, float range_m);
+
+/**
+ * Get the range for a certain direction
+ *
+ * @param direction Direction of the range
+ * @return Distance to an object in meter
+ */
+float rangeGet(rangeDirection_t direction);
+
+/**
+ * Enqueue a range measurement for distance to the ground in the current estimator.
+ *
+ * @param dstance Distance to the ground (m)
+ * @param stdDev The standard deviation of the range sample
+ * @param timeStamp The time when the range was sampled (in sys ticks)
+ * @return true if the sample was successfuly enqueued
+ */
+bool rangeEnqueueDownRangeInEstimator(float distance, float stdDev, uint32_t timeStamp);
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/sensfusion6.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/sensfusion6.h
new file mode 100644
index 0000000..11f1fc7
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/sensfusion6.h
@@ -0,0 +1,39 @@
+/**
+ * || ____ _ __ ______
+ * +------+ / __ )(_) /_/ ____/_________ _____ ___
+ * | 0xBC | / __ / / __/ / / ___/ __ `/_ / / _ \
+ * +------+ / /_/ / / /_/ /___ / / / /_/ / / /_/ __/
+ * || || /_____/_/\__/\____//_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2012 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef SENSORFUSION6_H_
+#define SENSORFUSION6_H_
+#include <stdbool.h>
+
+void sensfusion6Init(void);
+bool sensfusion6Test(void);
+
+void sensfusion6UpdateQ(float gx, float gy, float gz, float ax, float ay, float az, float dt);
+void sensfusion6GetQuaternion(float* qx, float* qy, float* qz, float* qw);
+void sensfusion6GetEulerRPY(float* roll, float* pitch, float* yaw);
+float sensfusion6GetAccZWithoutGravity(const float ax, const float ay, const float az);
+float sensfusion6GetInvThrustCompensationForTilt();
+
+#endif /* SENSORFUSION6_H_ */
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/sitaw.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/sitaw.h
new file mode 100644
index 0000000..13f1c45
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/sitaw.h
@@ -0,0 +1,81 @@
+/*
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ * sitaw.h - Interface to situation awareness.
+ */
+
+#ifndef __SITAW_H__
+#define __SITAW_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "stabilizer_types.h"
+
+void sitAwInit(void);
+void sitAwUpdateSetpoint(setpoint_t *setpoint, const sensorData_t *sensorData,
+ const state_t *state);
+/* Enable the situation awareness framework. */
+#define SITAW_ENABLED
+/* Enable the different functions of the situation awareness framework. */
+//#define SITAW_FF_ENABLED /* Uncomment to enable */
+//#define SITAW_AR_ENABLED /* Uncomment to enable */
+#define SITAW_TU_ENABLED /* Uncomment to enable */
+
+/* Configuration options for the 'Free Fall' detection. */
+#define SITAW_FF_THRESHOLD 0.1 /* The default tolerance for AccWZ deviations from -1, indicating Free Fall. */
+#define SITAW_FF_TRIGGER_COUNT 15 /* The number of consecutive tests for Free Fall to be detected. Configured for 250Hz testing. */
+
+/* Configuration options for the 'At Rest' detection. */
+#define SITAW_AR_THRESHOLD 0.05 /* The default tolerance for AccZ deviations from 1 and AccX, AccY deviations from 0, indicating At Rest. */
+#define SITAW_AR_TRIGGER_COUNT 500 /* The number of consecutive tests for At Rest to be detected. Configured for 250Hz testing. */
+
+/* Configuration options for the 'Tumbled' detection. */
+#define SITAW_TU_ACC_THRESHOLD (-0.5f) /* The maximum acc.z value indicating a Tumbled situation. */
+#define SITAW_TU_ACC_TRIGGER_COUNT 30 /* The number of consecutive tests for Tumbled to be detected. Configured for 250Hz testing. */
+#define SITAW_TU_IN_FLIGHT_THRESHOLD 1000 /* Minimum summed motor PWM that means we are flying */
+
+/* LOG configurations. Enable these to be able to log detection in the cfclient. */
+#define SITAW_LOG_ENABLED /* Uncomment to enable LOG framework. */
+//#define SITAW_FF_LOG_ENABLED /* Uncomment to enable LOG framework for the Free Fall detection trigger object. */
+//#define SITAW_AR_LOG_ENABLED /* Uncomment to enable LOG framework for the At Rest detection trigger object. */
+//#define SITAW_TU_LOG_ENABLED /* Uncomment to enable LOG framework for the Tumbled detection trigger object. */
+#define SITAW_LOG_ALL_DETECT_ENABLED /* Uncomment to enable LOG framework for all 'Detected' flags. */
+
+/* PARAM configurations. Enable these to be able to tweak detection configurations from the cfclient. */
+//#define SITAW_PARAM_ENABLED /* Uncomment to enable PARAM framework. */
+//#define SITAW_FF_PARAM_ENABLED /* Uncomment to enable PARAM framework for the Free Fall detection. */
+//#define SITAW_AR_PARAM_ENABLED /* Uncomment to enable PARAM framework for the At Rest detection. */
+//#define SITAW_TU_PARAM_ENABLED /* Uncomment to enable PARAM framework for the Tumbled detection. */
+
+#ifdef SITAW_FF_ENABLED
+bool sitAwFFDetected(void);
+#endif
+#ifdef SITAW_AR_ENABLED
+bool sitAwARDetected(void);
+#endif
+#ifdef SITAW_TU_ENABLED
+bool sitAwTuDetected(void);
+#endif
+
+#endif
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/sound.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/sound.h
new file mode 100644
index 0000000..59aecea
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/sound.h
@@ -0,0 +1,63 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ * sound.h - Module used to play melodies and system sounds though a buzzer
+ */
+
+#ifndef __SOUND_H__
+#define __SOUND_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#define SND_OFF 0
+#define FACTORY_TEST 1
+#define SND_USB_DISC 2
+#define SND_USB_CONN 3
+#define SND_BAT_FULL 4
+#define SND_BAT_LOW 5
+#define SND_STARTUP 6
+#define SND_CALIB 7
+
+/**
+ * Initialize sound sub-system.
+ */
+void soundInit(void);
+
+/**
+ * Test the sound sub-system.
+ */
+bool soundTest(void);
+
+/**
+ * Set the effect that should be played by the sound sub-system.
+ */
+void soundSetEffect(uint32_t effect);
+
+/**
+ * Manufally set a frequency that should be played by the sound syb-system.
+ */
+void soundSetFreq(uint32_t freq);
+
+#endif /* __SOUND_H__ */
+
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/stabilizer.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/stabilizer.h
new file mode 100644
index 0000000..8824c10
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/stabilizer.h
@@ -0,0 +1,68 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ * stabilizer.h: Stabilizer orchestrator
+ */
+#ifndef STABILIZER_H_
+#define STABILIZER_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "estimator.h"
+
+#define EMERGENCY_STOP_TIMEOUT_DISABLED (-1)
+
+/**
+ * Initialize the stabilizer subsystem and launch the stabilizer loop task.
+ * The stabilizer loop task will wait on systemWaitStart() before running.
+ */
+void stabilizerInit(StateEstimatorType estimator);
+
+/**
+ * Test the stabilizer subsystem. Calls test for all the stabilizer related
+ * sensors.
+ * @return True if all test has passed. False otherwise.
+ */
+bool stabilizerTest(void);
+
+/**
+ * Enable emergency stop, will shut-off energy to the motors.
+ */
+void stabilizerSetEmergencyStop();
+
+/**
+ * Disable emergency stop, will enable energy to the motors.
+ */
+void stabilizerResetEmergencyStop();
+
+/**
+ * Restart the countdown until emergercy stop will be enabled.
+ *
+ * @param timeout Timeout in stabilizer loop tick. The stabilizer loop rate is
+ * RATE_MAIN_LOOP.
+ */
+void stabilizerSetEmergencyStopTimeout(int timeout);
+
+
+#endif /* STABILIZER_H_ */
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/stabilizer_types.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/stabilizer_types.h
new file mode 100644
index 0000000..437faba
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/stabilizer_types.h
@@ -0,0 +1,276 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ * stabilizer.h: Stabilizer orchestrator
+ */
+#ifndef __STABILIZER_TYPES_H__
+#define __STABILIZER_TYPES_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "imu_types.h"
+//#include "lighthouse_calibration.h"
+
+/* Data structure used by the stabilizer subsystem.
+ * All have a timestamp to be set when the data is calculated.
+ */
+
+/** Attitude in euler angle form */
+typedef struct attitude_s {
+ uint32_t timestamp; // Timestamp when the data was computed
+
+ float roll;
+ float pitch;
+ float yaw;
+} attitude_t;
+
+/* vector */
+#define vec3d_size 3
+typedef float vec3d[vec3d_size];
+typedef float mat3d[vec3d_size][vec3d_size];
+
+/* x,y,z vector */
+struct vec3_s {
+ uint32_t timestamp; // Timestamp when the data was computed
+
+ float x;
+ float y;
+ float z;
+};
+
+typedef struct vec3_s vector_t;
+typedef struct vec3_s point_t;
+typedef struct vec3_s velocity_t;
+typedef struct vec3_s acc_t;
+
+/* Orientation as a quaternion */
+typedef struct quaternion_s {
+ uint32_t timestamp;
+
+ union {
+ struct {
+ float q0;
+ float q1;
+ float q2;
+ float q3;
+ };
+ struct {
+ float x;
+ float y;
+ float z;
+ float w;
+ };
+ };
+} quaternion_t;
+
+typedef struct tdoaMeasurement_s {
+ point_t anchorPosition[2];
+ float distanceDiff;
+ float stdDev;
+} tdoaMeasurement_t;
+
+typedef struct baro_s {
+ float pressure; // mbar
+ float temperature; // degree Celcius
+ float asl; // m (ASL = altitude above sea level)
+} baro_t;
+
+typedef struct positionMeasurement_s {
+ union {
+ struct {
+ float x;
+ float y;
+ float z;
+ };
+ float pos[3];
+ };
+ float stdDev;
+} positionMeasurement_t;
+
+typedef struct poseMeasurement_s {
+ union {
+ struct {
+ float x;
+ float y;
+ float z;
+ };
+ float pos[3];
+ };
+ quaternion_t quat;
+ float stdDevPos;
+ float stdDevQuat;
+} poseMeasurement_t;
+
+typedef struct distanceMeasurement_s {
+ union {
+ struct {
+ float x;
+ float y;
+ float z;
+ };
+ float pos[3];
+ };
+ float distance;
+ float stdDev;
+} distanceMeasurement_t;
+
+typedef struct zDistance_s {
+ uint32_t timestamp;
+ float distance; // m
+} zDistance_t;
+
+typedef struct sensorData_s {
+ Axis3f acc; // Gs
+ Axis3f gyro; // deg/s
+ Axis3f mag; // gauss
+ baro_t baro;
+#ifdef LOG_SEC_IMU
+ Axis3f accSec; // Gs
+ Axis3f gyroSec; // deg/s
+#endif
+ uint64_t interruptTimestamp;
+} sensorData_t;
+
+typedef struct state_s {
+ attitude_t attitude; // deg (legacy CF2 body coordinate system, where pitch is inverted)
+ quaternion_t attitudeQuaternion;
+ point_t position; // m
+ velocity_t velocity; // m/s
+ acc_t acc; // Gs (but acc.z without considering gravity)
+} state_t;
+
+typedef struct control_s {
+ int16_t roll;
+ int16_t pitch;
+ int16_t yaw;
+ float thrust;
+} control_t;
+
+typedef enum mode_e {
+ modeDisable = 0,
+ modeAbs,
+ modeVelocity
+} stab_mode_t;
+
+typedef struct setpoint_s {
+ uint32_t timestamp;
+
+ attitude_t attitude; // deg
+ attitude_t attitudeRate; // deg/s
+ quaternion_t attitudeQuaternion;
+ float thrust;
+ point_t position; // m
+ velocity_t velocity; // m/s
+ acc_t acceleration; // m/s^2
+ bool velocity_body; // true if velocity is given in body frame; false if velocity is given in world frame
+
+ struct {
+ stab_mode_t x;
+ stab_mode_t y;
+ stab_mode_t z;
+ stab_mode_t roll;
+ stab_mode_t pitch;
+ stab_mode_t yaw;
+ stab_mode_t quat;
+ } mode;
+} setpoint_t;
+
+/** Estimate of position */
+typedef struct estimate_s {
+ uint32_t timestamp; // Timestamp when the data was computed
+
+ point_t position;
+} estimate_t;
+
+/** Setpoint for althold */
+typedef struct setpointZ_s {
+ float z;
+ bool isUpdate; // True = small update of setpoint, false = completely new
+} setpointZ_t;
+
+/** Flow measurement**/
+typedef struct flowMeasurement_s {
+ uint32_t timestamp;
+ union {
+ struct {
+ float dpixelx; // Accumulated pixel count x
+ float dpixely; // Accumulated pixel count y
+ };
+ float dpixel[2]; // Accumulated pixel count
+ };
+ float stdDevX; // Measurement standard deviation
+ float stdDevY; // Measurement standard deviation
+ float dt; // Time during which pixels were accumulated
+} flowMeasurement_t;
+
+
+/** TOF measurement**/
+typedef struct tofMeasurement_s {
+ uint32_t timestamp;
+ float distance;
+ float stdDev;
+} tofMeasurement_t;
+
+/** Absolute height measurement */
+typedef struct heightMeasurement_s {
+ uint32_t timestamp;
+ float height;
+ float stdDev;
+} heightMeasurement_t;
+
+/** Yaw error measurement */
+typedef struct {
+ uint32_t timestamp;
+ float yawError;
+ float stdDev;
+} yawErrorMeasurement_t;
+
+/** Sweep angle measurement */
+// typedef struct {
+// uint32_t timestamp;
+// const vec3d* sensorPos; // Sensor position in the CF reference frame
+// const vec3d* rotorPos; // Pos of rotor origin in global reference frame
+// const mat3d* rotorRot; // Rotor rotation matrix
+// const mat3d* rotorRotInv; // Inverted rotor rotation matrix
+// float t; // t is the tilt angle of the light plane on the rotor
+// float measuredSweepAngle;
+// float stdDev;
+// const lighthouseCalibrationSweep_t* calib;
+// lighthouseCalibrationMeasurementModel_t calibrationMeasurementModel;
+// } sweepAngleMeasurement_t;
+
+// Frequencies to bo used with the RATE_DO_EXECUTE_HZ macro. Do NOT use an arbitrary number.
+#define RATE_1000_HZ 1000
+#define RATE_500_HZ 500
+#define RATE_250_HZ 250
+#define RATE_100_HZ 100
+#define RATE_50_HZ 50
+#define RATE_25_HZ 25
+
+#define RATE_MAIN_LOOP RATE_1000_HZ
+#define ATTITUDE_RATE RATE_500_HZ
+#define POSITION_RATE RATE_100_HZ
+
+#define RATE_DO_EXECUTE(RATE_HZ, TICK) ((TICK % (RATE_MAIN_LOOP / RATE_HZ)) == 0)
+
+#endif
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/static_mem.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/static_mem.h
new file mode 100644
index 0000000..7ed4319
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/static_mem.h
@@ -0,0 +1,214 @@
+/**
+ * ,---------, ____ _ __
+ * | ,-^-, | / __ )(_) /_______________ _____ ___
+ * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2019 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @file static_mem.h
+ * @brief Utility macros to create static memory for OS objects such as
+ * queues, semaphores and so on.
+ *
+ * @copyright Copyright (c) 2019
+ *
+ */
+
+#pragma once
+
+#include "cfassert.h"
+
+#define CCM_NOT_SUPPORTED
+
+/**
+ * @brief Macro to indicate that a variable can be placed in the CCM
+ * (Core Coupled Memroy) by the linker, instead of normal RAM.
+ * The CCM has some special properties and can
+ * not be used for DMA transfers, why special care should be taken to
+ * make sure pointer to this memory will not be passed to a function
+ * doing DMA.
+ *
+ * The memory is zero initialized at start up. If you assign a value
+ * to the variable when declared, this value will be silently ignored
+ * without a warning!
+ *
+ * Note: Using this macro does not guarantee that the variable will
+ * end up in the CCM. The current implementation puts is in CCM but
+ * that might change later.
+ */
+#if defined(UNIT_TEST_MODE)
+ #define NO_DMA_CCM_SAFE_ZERO_INIT
+#elif defined(CCM_NOT_SUPPORTED)
+ #define NO_DMA_CCM_SAFE_ZERO_INIT
+#else
+ #define NO_DMA_CCM_SAFE_ZERO_INIT __attribute__((section(".ccmbss")))
+#endif
+
+/**
+ * @brief Macro to force a variable to be placed in the CCM
+ * (Core Coupled Memroy) by the linker, instead of normal RAM.
+ * The CCM has some special properties and can
+ * not be used for DMA transfers, why special care should be taken to
+ * make sure pointer to this memory will not be passed to a function
+ * doing DMA.
+ *
+ * The memory is zero initialized at start up. If you assign a value
+ * to the variable when declared, this value will be silently ignored
+ * without a warning!
+ */
+#if defined(UNIT_TEST_MODE)
+ #define FORCE_CCM_ZERO_INIT
+#elif defined(CCM_NOT_SUPPORTED)
+ #define FORCE_CCM_ZERO_INIT
+#else
+ #define FORCE_CCM_ZERO_INIT __attribute__((section(".ccmbss")))
+#endif
+
+
+/**
+ * @brief Creation of queues using static memory.
+ *
+ * STATIC_MEM_QUEUE_ALLOC() and STATIC_MEM_QUEUE_CREATE() are used togehter to set up
+ * and create a queue. STATIC_MEM_QUEUE_ALLOC() defines a number of static variables,
+ * that are required by the queue. STATIC_MEM_QUEUE_CREATE() creates the OS queue object
+ * using the previously defined variables.
+ *
+ * Example:
+ * static xQueueHandle myQueue;
+ * STATIC_MEM_QUEUE_ALLOC(myQueue, 5, sizeof(int));
+ * // ...
+ * void init() {
+ * myQueue = STATIC_MEM_QUEUE_CREATE(myQueue);
+ * // myQueue can now be used in OS calls
+ * }
+ *
+ * Note: the queue handle may have the same name as used in the macros, but it is
+ * not necessary.
+ */
+
+/**
+ * @brief Creates a number of variables required for a queue using static memory.
+ *
+ * All variables created by this macreo are named using the NAME parameter as a base.
+ *
+ * Example:
+ * STATIC_MEM_QUEUE_ALLOC(myQueue, 5, sizeof(int));
+ *
+ * expands to:
+ * // static const int osSys_myQueueLength = 5;
+ * // static const int osSys_myQueueItemSize = sizeof(int);
+ * // static uint8_t osSys_myQueueStorage[5 * sizeof(int)];
+ * // static StaticQueue_t osSys_myQueueSMgm;
+ *
+ * Note: the memory is allocated in CCM RAM. Read/write to the queue is done by copy
+ * and the special properties of CCM RAM should not have any impact on the behaviour.
+ *
+ * @param NAME - the name of the queue handle (xQueueHandle). The name is also used as
+ * base name for the other variables that are required.
+ * @param LENGTH - the length of the queue (in items)
+ * @param ITEM_SIZE - the size of the items in the queue
+ */
+#define STATIC_MEM_QUEUE_ALLOC(NAME, LENGTH, ITEM_SIZE)\
+ static const int osSys_ ## NAME ## Length = (LENGTH); \
+ static const int osSys_ ## NAME ## ItemSize = (ITEM_SIZE); \
+ NO_DMA_CCM_SAFE_ZERO_INIT static uint8_t osSys_ ## NAME ## Storage[(LENGTH) * (ITEM_SIZE)]; \
+ NO_DMA_CCM_SAFE_ZERO_INIT static StaticQueue_t osSys_ ## NAME ## Mgm;
+
+/**
+ * @brief Creates a queue using static memory
+ *
+ * The queue is created under the assumption that variables have been created
+ * using the STATIC_MEM_QUEUE_ALLOC() macro
+ *
+ * Example:
+ * STATIC_MEM_QUEUE_CREATE(myQueue);
+ * // Exoands to
+ * // xQueueCreateStatic(osSys_myQueueLength, osSys_myQueueItemSize, osSys_myQueueStorage, &osSys_myQueueMgm);
+ *
+ * @param NAME - the name of the queue handle
+ */
+#define STATIC_MEM_QUEUE_CREATE(NAME) xQueueCreateStatic(osSys_ ## NAME ## Length, osSys_ ## NAME ## ItemSize, osSys_ ## NAME ## Storage, &osSys_ ## NAME ## Mgm)
+
+
+/**
+ * @brief Creation of tasks using static memory.
+ *
+ * STATIC_MEM_TASK_ALLOC() and STATIC_MEM_TASK_CREATE() are used together to
+ * allocate buffers and create a task. STATIC_MEM_TASK_ALLOC() defines the
+ * required memory and variables while STATIC_MEM_TASK_CREATE() creats
+ * the task. The NAME is used as a base name for the necessary variables and
+ * does not have to be the same as FUNCTION or TASK_NAME, but that works as well.
+ *
+ * Example:
+ * static TaskHandle_t taskHandle;
+ * STATIC_MEM_TASK_ALLOC(myTask, 100);
+ *
+ * void taskFcn(void *arg) {
+ * // Do stuff...
+ * }
+ *
+ * void init() {
+ * taskHandle = STATIC_MEM_TASK_CREATE(myTask, taskFcn, "MY_TASK", NULL, 1);
+ * // ...
+ * }
+ */
+
+/**
+ * @brief Allocate variables and stack for a task using static memory.
+ *
+ * Note: the stack memory is allocated in normal RAM while the buffers
+ * are allocated in CCM RAM. The special properties of CCM RAM should not
+ * have any impact on the behaviour.
+
+ * @param NAME A name used as base name for the variables that are created
+ * @param STACK_DEPTH The stack depth in nr of StackType_t entries.
+ */
+#define STATIC_MEM_TASK_ALLOC(NAME, STACK_DEPTH) \
+ static const int osSys_ ## NAME ## StackDepth = (STACK_DEPTH); \
+ static StackType_t osSys_ ## NAME ## StackBuffer[(STACK_DEPTH)]; \
+ NO_DMA_CCM_SAFE_ZERO_INIT static StaticTask_t osSys_ ## NAME ## TaskBuffer;
+
+/**
+ * @brief Allocate variables and stack for a task using static memory.
+
+ * All memory, including the stack is allocated in CCM RAM which means
+ * that it can not be used for DMA transfers. Some drivers may use DMA
+ * to retrieve data from a peripheral and it will NOT be possible to
+ * pass a pointer from this stack to such driver
+ *
+ * @param NAME A name used as base name for the variables that are created
+ * @param STACK_DEPTH The stack depth in nr of StackType_t entries.
+ */
+#define STATIC_MEM_TASK_ALLOC_STACK_NO_DMA_CCM_SAFE(NAME, STACK_DEPTH) \
+ static const int osSys_ ## NAME ## StackDepth = (STACK_DEPTH); \
+ NO_DMA_CCM_SAFE_ZERO_INIT static StackType_t osSys_ ## NAME ## StackBuffer[(STACK_DEPTH)]; \
+ NO_DMA_CCM_SAFE_ZERO_INIT static StaticTask_t osSys_ ## NAME ## TaskBuffer;
+
+/**
+ * @brief Create a task using static memory
+ *
+ * The task is created under the assumption that STATIC_MEM_TASK_ALLOC() has been
+ * used to define the required variables and buffers.
+ *
+ * @param NAME A name used as base name for the variables, same name that was used in STATIC_MEM_TASK_ALLOC()
+ * @param FUNCTION The function that implements the task
+ * @param TASK_NAME A descriptive name for the task
+ * @param PARAMETERS Passed on as argument to the function implementing the task
+ * @param PRIORITY The task priority
+ */
+#define STATIC_MEM_TASK_CREATE(NAME, FUNCTION, TASK_NAME, PARAMETERS, PRIORITY) xTaskCreateStatic((FUNCTION), (TASK_NAME), osSys_ ## NAME ## StackDepth, (PARAMETERS), (PRIORITY), osSys_ ## NAME ## StackBuffer, &osSys_ ## NAME ## TaskBuffer)
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/sysload.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/sysload.h
new file mode 100644
index 0000000..1381e0f
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/sysload.h
@@ -0,0 +1,6 @@
+#ifndef __SYSLOAD_H__
+#define __SYSLOAD_H__
+
+void sysLoadInit();
+
+#endif
\ No newline at end of file
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/system.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/system.h
new file mode 100644
index 0000000..32cf4ab
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/system.h
@@ -0,0 +1,46 @@
+/*
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ * system.h - Top level module header file
+ */
+
+#ifndef __SYSTEM_H__
+#define __SYSTEM_H__
+
+#include <stdbool.h>
+#include <stdint.h>
+
+void systemInit(void);
+bool systemTest(void);
+
+void systemLaunch(void);
+
+
+void systemStart();
+void systemWaitStart(void);
+void systemSetCanFly(bool val);
+bool systemCanFly(void);
+void systemSetArmed(bool val);
+bool systemIsArmed();
+
+#endif //__SYSTEM_H__
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/tdoaEngineInstance.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/tdoaEngineInstance.h
new file mode 100644
index 0000000..73428ff
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/tdoaEngineInstance.h
@@ -0,0 +1,33 @@
+
+/**
+ * ,---------, ____ _ __
+ * | ,-^-, | / __ )(_) /_______________ _____ ___
+ * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+ * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+ * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2020 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/>.
+ *
+ *
+ * tdoaEngineInstance.h - exposes the tdoa engine state instance
+ */
+
+#pragma once
+
+#include "tdoaEngine.h"
+
+extern tdoaEngineState_t tdoaEngineState;
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/trigger.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/trigger.h
new file mode 100644
index 0000000..2ce1074
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/trigger.h
@@ -0,0 +1,69 @@
+/*
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ * trigger.h - Interface to trigger mechanism.
+ */
+
+#ifndef __TRIGGER_H__
+#define __TRIGGER_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/* Trigger function type. */
+typedef enum {
+ triggerFuncNone = 0, /* No trigger function. */
+ triggerFuncIsLE = 1, /* Increases testCounter if test value is Less than or Equal to threshold value. */
+ triggerFuncIsGE = 2, /* Increases testCounter if test value is Greater than or Equal to threshold value. */
+} triggerFunc_t;
+
+/* Trigger handler function type. */
+typedef void (*triggerHandler_t)(void *);
+
+/**
+ * Trigger object.
+ *
+ * This object is published here so that users may investigate the testCounter and released attributes
+ * during debugging. Such attributes can for instance be passed through the LOG framework.
+ */
+typedef struct {
+ bool active; /* Indicates if the trigger is active or not. */
+ triggerFunc_t func; /* The trigger function type. */
+ float threshold; /* The threshold the test value is compared against using the trigger function. */
+ uint32_t triggerCount; /* When testCounter reaches this value, a trigger is released. */
+ triggerHandler_t handler; /* If registered, the handler function is called when the trigger is released. */
+ bool handlerCalled; /* Indicates if the handler has been called. The handler is only called once after the trigger is released. */
+ void *handlerArg; /* The argument to pass to the handler function. */
+ uint32_t testCounter; /* As long as smaller than triggerCount, this counter is incremented for each consecutive test the tested value is within the threshold. */
+ bool released; /* Indicates if the trigger has been released (true) or not (false). */
+} trigger_t;
+
+/* Trigger functionality. */
+void triggerInit(trigger_t *trigger, triggerFunc_t func, float threshold, uint32_t triggerCount);
+void triggerRegisterHandler(trigger_t *trigger, triggerHandler_t handler, void *handlerArg);
+void triggerDeInit(trigger_t *trigger);
+void triggerActivate(trigger_t *trigger, bool active);
+void triggerReset(trigger_t *trigger);
+bool triggerTestValue(trigger_t *trigger, float testValue);
+
+#endif
diff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/worker.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/worker.h
new file mode 100644
index 0000000..63d5dba
--- /dev/null
+++ b/Firmware/esp-drone/components/core/crazyflie/modules/interface/worker.h
@@ -0,0 +1,54 @@
+/**
+ * || ____ _ __
+ * +------+ / __ )(_) /_______________ _____ ___
+ * | 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/>.
+ *
+ * worker.h - Worker system that can execute asynchronous actions in tasks
+ */
+#ifndef __WORKER_H
+#define __WORKER_H
+
+#include <stdbool.h>
+
+void workerInit();
+
+bool workerTest();
+
+/**
+ * Light printf implementation
+ *
+ * This function exectute the worker loop and never returns except if the worker
+ * module has not been initialized.
+ */
+void workerLoop();
+
+/**
+ * Schedule a function for execution by the worker loop
+ * The function will be executed as soon as possible by the worker loop.
+ * Scheduled functions are stacked in a FIFO queue.
+ *
+ * @param function Function to be executed
+ * @param arg Argument that will be passed to the function when executed
+ * @return 0 in case of success. Anything else on failure.
+ */
+int workerSchedule(void (*function)(void*), void *arg);
+
+#endif //__WORKER_H