Migrating to new firmware

.gitignore

@@ -1,4 +1,5 @@
 build/
 *.cmd
+*.tmp
 *.o
 *:Zone.Identifier

.gitmodules

@@ -1,6 +1,9 @@
+[submodule "apps/eigen_test/Eigen"]
+	path = apps/eigen_test/Eigen
+	url = https://gitlab.com/libeigen/eigen.git
+[submodule "TinyMPC"]
+	path = TinyMPC
+	url = https://github.com/moisesmata/TinyMPC
 [submodule "crazyflie-firmware"]
 	path = crazyflie-firmware
-	url = https://github.com/bitcraze/crazyflie-firmware.git
-[submodule "TinyMPC"]
-	path = src/TinyMPC
-	url = https://github.com/moisesmata/TinyMPC.git
+	url = https://github.com/moisesmata/crazyflie-firmware.git

apps/controller_tinympc_eigen/.gitignore

@@ -0,0 +1,3 @@
+bin/*
+cf2.*
+build/*

apps/controller_tinympc_eigen/Kbuild

@@ -0,0 +1 @@
+obj-y += src/

apps/controller_tinympc_eigen/Makefile

@@ -0,0 +1,38 @@
+# The firmware uses the Kbuild build system. There are 'Kbuild' files in this
+# example that outlays what needs to be built. (check src/Kbuild).
+#
+# The firmware is configured using options in Kconfig files, the
+# values of these end up in the .config file in the firmware directory.
+#
+# By setting the OOT_CONFIG (it is '$(PWD)/oot-config' by default) environment
+# variable you can provide a custom configuration. It is important that you
+# enable the app-layer. See app-config in this directory for example.
+
+#
+# We want to execute the main Makefile for the firmware project,
+# it will handle the build for us.
+#
+CRAZYFLIE_BASE := ../../crazyflie-firmware
+
+#
+# To include header files from other directories
+#
+# EXTRA_CFLAGS += -lstdc++
+# EXTRA_CFLAGS += -lgcc
+# EXTRA_CFLAGS += -lc
+# EXTRA_CFLAGS += -lm
+EXTRA_CFLAGS += -I$(PWD)/TinyMPC-ADMM/ext/Eigen
+EXTRA_CFLAGS += -I$(PWD)/TinyMPC-ADMM/src
+EXTRA_CFLAGS += -DEIGEN_INITIALIZE_MATRICES_BY_ZERO
+EXTRA_CFLAGS += -DEIGEN_NO_MALLOC
+EXTRA_CFLAGS += -DNDEBUG
+EXTRA_CFLAGS += -Wno-unused-result
+#
+# We override the default OOT_CONFIG here, we could also name our config
+# to oot-config and that would be the default.
+#
+OOT_CONFIG := $(PWD)/app-config
+
+OOT_USES_CXX := 1
+
+include $(CRAZYFLIE_BASE)/tools/make/oot.mk

apps/controller_tinympc_eigen/README.md

@@ -0,0 +1,44 @@
+# TinyMPC-ADMM Controller for Crazyflie
+
+This is a TinyMPC-ADMM based controller for the Crazyflie, adapted for the latest firmware API. It's based on the original implementation by Ishaan's team but updated to work with the new crazyflie firmware structure.
+
+## Features
+
+- Uses TinyMPC-ADMM for Model Predictive Control
+- Supports trajectory tracking and setpoint following
+- Eigen-based matrix operations for efficient computation
+- Configurable solver parameters and trajectory options
+
+## Key Changes from Original Implementation
+
+1. Updated controller function signature to use `stabilizerStep_t` instead of `uint32_t tick`
+2. Adapted for new firmware build system and structure
+3. Maintained compatibility with TinyMPC-ADMM library
+4. Added proper includes for new firmware API (`stabilizer_types.h`)
+
+## Build Instructions
+
+1. Navigate to this directory
+2. Run `make` to build the controller
+3. Flash the resulting firmware to your Crazyflie
+
+## Configuration
+
+- Trajectory parameters and model matrices are defined in header files (e.g., `params_500hz.h`, `traj_fig8_12.h`)
+- Controller parameters can be adjusted in the main controller file
+- MPC solver settings (iterations, tolerances) are configurable
+
+## Logging
+
+The controller provides several logging variables for monitoring:
+- `ctrlMPC.iters`: Number of solver iterations
+- `ctrlMPC.mpcTime`: Solver execution time in microseconds
+- `ctrlMPC.primal_residual`: Primal residual from ADMM solver
+- `ctrlMPC.dual_residual`: Dual residual from ADMM solver
+- `ctrlMPC.ref_x/y/z`: Reference trajectory coordinates
+
+## Notes
+
+- The controller runs at 500Hz by default
+- Trajectory execution can be enabled/disabled via `en_traj` parameter
+- Hover values are preconfigured for specific Crazyflie units and may need adjustment

apps/controller_tinympc_eigen/TinyMPC-ADMM/LICENSE.md

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2023 Robotic Exploration Lab
+
+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.

apps/controller_tinympc_eigen/TinyMPC-ADMM/README.md

@@ -0,0 +1 @@
+# TinyMPC-ADMM

apps/controller_tinympc_eigen/TinyMPC-ADMM/ext/Eigen/Eigen.h

@@ -0,0 +1,82 @@
+/*
+Eigen.h
+Brian R Taylor
+brian.taylor@bolderflight.com
+2017-02-08
+
+Copyright (c) 2017 Bolder Flight Systems
+
+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.
+*/
+
+// Credits:   @rpavlik for writing the original header for the Eigen313 library, which this
+// was derived from:
+// http://forum.arduino.cc/index.php?PHPSESSID=a86gv50nb3e3ireijfmli63260&topic=144446.msg1089371#msg1089371
+
+// Disable debug asserts.
+#define EIGEN_NO_DEBUG 1
+
+// Hint to number of registers
+#define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 16
+
+#ifdef A0
+# define NEED_A0_RESTORED A0
+# undef A0
+#endif
+#ifdef A1
+# define NEED_A1_RESTORED A1
+# undef A1
+#endif
+#ifdef B0
+# define NEED_B0_RESTORED B0
+# undef B0
+#endif
+#ifdef B1
+# define NEED_B1_RESTORED B1
+# undef B1
+#endif
+#ifdef round
+# define NEED_round_RESTORED round
+# undef round
+#endif
+
+namespace std {
+	struct nothrow_t;
+}
+
+// Include main EIGEN Core header
+#include <Eigen/Core>
+
+#ifdef NEED_A0_RESTORED
+# define A0 NEED_A0_RESTORED
+# undef NEED_A0_RESTORED
+#endif
+#ifdef NEED_A1_RESTORED
+# define A1 NEED_A1_RESTORED
+# undef NEED_A1_RESTORED
+#endif
+#ifdef NEED_B0_RESTORED
+# define B0 NEED_B0_RESTORED
+# undef NEED_B0_RESTORED
+#endif
+#ifdef NEED_B1_RESTORED
+# define B1 NEED_B1_RESTORED
+# undef NEED_B1_RESTORED
+#endif
+#ifdef NEED_round_RESTORED
+# define round NEED_round_RESTORED
+# undef NEED_round_RESTORED
+#endif

apps/controller_tinympc_eigen/TinyMPC-ADMM/ext/Eigen/Eigen/CMakeLists.txt

@@ -0,0 +1,19 @@
+include(RegexUtils)
+test_escape_string_as_regex()
+
+file(GLOB Eigen_directory_files "*")
+
+escape_string_as_regex(ESCAPED_CMAKE_CURRENT_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}")
+
+foreach(f ${Eigen_directory_files})
+  if(NOT f MATCHES "\\.txt" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/[.].+" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/src")
+    list(APPEND Eigen_directory_files_to_install ${f})
+  endif()
+endforeach(f ${Eigen_directory_files})
+
+install(FILES
+  ${Eigen_directory_files_to_install}
+  DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel
+  )
+
+install(DIRECTORY src DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel FILES_MATCHING PATTERN "*.h")

apps/controller_tinympc_eigen/TinyMPC-ADMM/ext/Eigen/Eigen/Cholesky

@@ -0,0 +1,46 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_CHOLESKY_MODULE_H
+#define EIGEN_CHOLESKY_MODULE_H
+
+#include "Core"
+#include "Jacobi"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** \defgroup Cholesky_Module Cholesky module
+  *
+  *
+  *
+  * This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices.
+  * Those decompositions are also accessible via the following methods:
+  *  - MatrixBase::llt()
+  *  - MatrixBase::ldlt()
+  *  - SelfAdjointView::llt()
+  *  - SelfAdjointView::ldlt()
+  *
+  * \code
+  * #include <Eigen/Cholesky>
+  * \endcode
+  */
+
+#include "src/Cholesky/LLT.h"
+#include "src/Cholesky/LDLT.h"
+#ifdef EIGEN_USE_LAPACKE
+#ifdef EIGEN_USE_MKL
+#include "mkl_lapacke.h"
+#else
+#include "src/misc/lapacke.h"
+#endif
+#include "src/Cholesky/LLT_LAPACKE.h"
+#endif
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_CHOLESKY_MODULE_H
+/* vim: set filetype=cpp et sw=2 ts=2 ai: */

apps/controller_tinympc_eigen/TinyMPC-ADMM/ext/Eigen/Eigen/CholmodSupport

@@ -0,0 +1,48 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_CHOLMODSUPPORT_MODULE_H
+#define EIGEN_CHOLMODSUPPORT_MODULE_H
+
+#include "SparseCore"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+extern "C" {
+  #include <cholmod.h>
+}
+
+/** \ingroup Support_modules
+  * \defgroup CholmodSupport_Module CholmodSupport module
+  *
+  * This module provides an interface to the Cholmod library which is part of the <a href="http://www.suitesparse.com">suitesparse</a> package.
+  * It provides the two following main factorization classes:
+  * - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization.
+  * - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial).
+  *
+  * For the sake of completeness, this module also propose the two following classes:
+  * - class CholmodSimplicialLLT
+  * - class CholmodSimplicialLDLT
+  * Note that these classes does not bring any particular advantage compared to the built-in
+  * SimplicialLLT and SimplicialLDLT factorization classes.
+  *
+  * \code
+  * #include <Eigen/CholmodSupport>
+  * \endcode
+  *
+  * In order to use this module, the cholmod headers must be accessible from the include paths, and your binary must be linked to the cholmod library and its dependencies.
+  * The dependencies depend on how cholmod has been compiled.
+  * For a cmake based project, you can use our FindCholmod.cmake module to help you in this task.
+  *
+  */
+
+#include "src/CholmodSupport/CholmodSupport.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_CHOLMODSUPPORT_MODULE_H
+