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.hnew file mode 100644index 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 filediff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/app_channel.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/app_channel.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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_Hdiff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/queuemonitor.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/queuemonitor.hnew file mode 100644index 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 filediff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/range.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/range.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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++#endifdiff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/sound.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/sound.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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)++#endifdiff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/static_mem.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/static_mem.hnew file mode 100644index 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.hnew file mode 100644index 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 filediff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/system.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/system.hnew file mode 100644index 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.hnew file mode 100644index 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.hnew file mode 100644index 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);++#endifdiff --git a/Firmware/esp-drone/components/core/crazyflie/modules/interface/worker.h b/Firmware/esp-drone/components/core/crazyflie/modules/interface/worker.hnew file mode 100644index 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