added most of the requirements needed for the structure of the QP in …

Makefile

@@ -59,6 +59,7 @@ INCLUDES += -I$(LIB)/STM32_USB_Device_Library/Core/inc
 INCLUDES += -I$(LIB)/STM32_USB_OTG_Driver/inc
 INCLUDES += -I$(LIB)/STM32F4xx_StdPeriph_Driver/inc
 INCLUDES += -I$(LIB)/vl53l1 -I$(LIB)/vl53l1/core/inc
+INCLUDES += -I$(LIB)/osqp/include
 INCLUDES += -I$(KBUILD_OUTPUT)/include/generated
 
 # Here we tell Kbuild where to look for Kbuild files which will tell the

bindings/setup.py

@@ -12,6 +12,7 @@
     "build/include/generated",
     "src/config",
     "src/drivers/interface",
+    "src/lib/osqp/include",
 ]
 
 fw_sources = [
@@ -30,6 +31,17 @@
     "src/modules/src/controller_sjc.c",
     "src/modules/src/controller_lee.c",
     "src/modules/src/controller_lee_payload.c",
+    "src/lib/osqp/src/osqp/auxil.c",
+    "src/lib/osqp/src/osqp/error.c",
+    "src/lib/osqp/src/osqp/kkt.c",
+    "src/lib/osqp/src/osqp/lin_alg.c",
+    "src/lib/osqp/src/osqp/osqp.c",
+    "src/lib/osqp/src/osqp/proj.c",
+    "src/lib/osqp/src/osqp/qdldl_interface.c",
+    "src/lib/osqp/src/osqp/qdldl.c",
+    "src/lib/osqp/src/osqp/scaling.c",
+    "src/lib/osqp/src/osqp/util.c",
+    "src/lib/osqp/src/osqp/workspace.c",
 ]
 
 cffirmware = Extension(

src/config/config.h

@@ -166,7 +166,7 @@
 #define MEM_TASK_STACKSIZE            (2 * configMINIMAL_STACK_SIZE)
 #define PARAM_TASK_STACKSIZE          (2 * configMINIMAL_STACK_SIZE)
 #define SENSORS_TASK_STACKSIZE        (2 * configMINIMAL_STACK_SIZE)
-#define STABILIZER_TASK_STACKSIZE     (3 * configMINIMAL_STACK_SIZE)
+#define STABILIZER_TASK_STACKSIZE     (6 * configMINIMAL_STACK_SIZE)
 #define NRF24LINK_TASK_STACKSIZE      configMINIMAL_STACK_SIZE
 #define ESKYLINK_TASK_STACKSIZE       configMINIMAL_STACK_SIZE
 #define SYSLINK_TASK_STACKSIZE        (2 * configMINIMAL_STACK_SIZE)

src/lib/Kbuild

@@ -44,3 +44,16 @@ obj-y += vl53l1/core/src/vl53l1_error_strings.o
 obj-y += vl53l1/core/src/vl53l1_register_funcs.o
 obj-y += vl53l1/core/src/vl53l1_silicon_core.o
 obj-y += vl53l1/core/src/vl53l1_wait.o
+
+# osqp
+obj-y += osqp/src/osqp/auxil.o
+obj-y += osqp/src/osqp/error.o
+obj-y += osqp/src/osqp/kkt.o
+obj-y += osqp/src/osqp/lin_alg.o
+obj-y += osqp/src/osqp/osqp.o
+obj-y += osqp/src/osqp/proj.o
+obj-y += osqp/src/osqp/qdldl_interface.o
+obj-y += osqp/src/osqp/qdldl.o
+obj-y += osqp/src/osqp/scaling.o
+obj-y += osqp/src/osqp/util.o
+obj-y += osqp/src/osqp/workspace.o

src/lib/osqp/include/auxil.h

@@ -0,0 +1,181 @@
+#ifndef AUXIL_H
+# define AUXIL_H
+
+# ifdef __cplusplus
+extern "C" {
+# endif // ifdef __cplusplus
+
+# include "types.h"
+
+
+/***********************************************************
+* Auxiliary functions needed to compute ADMM iterations * *
+***********************************************************/
+# if EMBEDDED != 1
+
+/**
+ * Compute rho estimate from residuals
+ * @param work Workspace
+ * @return     rho estimate
+ */
+c_float compute_rho_estimate(OSQPWorkspace *work);
+
+/**
+ * Adapt rho value based on current unscaled primal/dual residuals
+ * @param work Workspace
+ * @return     Exitflag
+ */
+c_int   adapt_rho(OSQPWorkspace *work);
+
+/**
+ * Set values of rho vector based on constraint types
+ * @param work Workspace
+ */
+void    set_rho_vec(OSQPWorkspace *work);
+
+/**
+ * Update values of rho vector based on updated constraints.
+ * If the constraints change, update the linear systems solver.
+ *
+ * @param work Workspace
+ * @return     Exitflag
+ */
+c_int   update_rho_vec(OSQPWorkspace *work);
+
+# endif // EMBEDDED
+
+/**
+ * Swap c_float vector pointers
+ * @param a first vector
+ * @param b second vector
+ */
+void swap_vectors(c_float **a,
+                  c_float **b);
+
+
+/**
+ * Cold start workspace variables xz and y
+ * @param work Workspace
+ */
+void cold_start(OSQPWorkspace *work);
+
+
+/**
+ * Update x_tilde and z_tilde variable (first ADMM step)
+ * @param work [description]
+ */
+void update_xz_tilde(OSQPWorkspace *work);
+
+
+/**
+ * Update x (second ADMM step)
+ * Update also delta_x (For for dual infeasibility)
+ * @param work Workspace
+ */
+void update_x(OSQPWorkspace *work);
+
+
+/**
+ * Update z (third ADMM step)
+ * @param work Workspace
+ */
+void update_z(OSQPWorkspace *work);
+
+
+/**
+ * Update y variable (fourth ADMM step)
+ * Update also delta_y to check for primal infeasibility
+ * @param work Workspace
+ */
+void update_y(OSQPWorkspace *work);
+
+
+/**
+ * Compute objective function from data at value x
+ * @param  work OSQPWorkspace structure
+ * @param  x    Value x
+ * @return      Objective function value
+ */
+c_float compute_obj_val(OSQPWorkspace *work,
+                        c_float       *x);
+
+/**
+ * Check whether QP has solution
+ * @param info OSQPInfo
+ */
+c_int has_solution(OSQPInfo *info);
+
+/**
+ * Store the QP solution
+ * @param work Workspace
+ */
+void store_solution(OSQPWorkspace *work);
+
+
+/**
+ * Update solver information
+ * @param work               Workspace
+ * @param iter               Iteration number
+ * @param compute_objective  Boolean (if compute the objective or not)
+ * @param polish             Boolean (if called from polish)
+ */
+void update_info(OSQPWorkspace *work,
+                 c_int          iter,
+                 c_int          compute_objective,
+                 c_int          polish);
+
+
+/**
+ * Reset solver information (after problem updates)
+ * @param info               Information structure
+ */
+void reset_info(OSQPInfo *info);
+
+
+/**
+ * Update solver status (value and string)
+ * @param info OSQPInfo
+ * @param status_val new status value
+ */
+void update_status(OSQPInfo *info,
+                   c_int     status_val);
+
+
+/**
+ * Check if termination conditions are satisfied
+ * If the boolean flag is ON, it checks for approximate conditions (10 x larger
+ * tolerances than the ones set)
+ *
+ * @param  work        Workspace
+ * @param  approximate Boolean
+ * @return      Residuals check
+ */
+c_int check_termination(OSQPWorkspace *work,
+                        c_int          approximate);
+
+
+# ifndef EMBEDDED
+
+/**
+ * Validate problem data
+ * @param  data OSQPData to be validated
+ * @return      Exitflag to check
+ */
+c_int validate_data(const OSQPData *data);
+
+
+/**
+ * Validate problem settings
+ * @param  settings OSQPSettings to be validated
+ * @return          Exitflag to check
+ */
+c_int validate_settings(const OSQPSettings *settings);
+
+
+# endif // #ifndef EMBEDDED
+
+# ifdef __cplusplus
+}
+# endif // ifdef __cplusplus
+
+#endif // ifndef AUXIL_H

src/lib/osqp/include/constants.h

@@ -0,0 +1,129 @@
+#ifndef CONSTANTS_H
+# define CONSTANTS_H
+
+# ifdef __cplusplus
+extern "C" {
+# endif // ifdef __cplusplus
+
+
+/*******************
+* OSQP Versioning *
+*******************/
+# define OSQP_VERSION ("0.6.2") /* string literals automatically null-terminated
+                                   */
+
+/******************
+* Solver Status  *
+******************/
+# define OSQP_DUAL_INFEASIBLE_INACCURATE (4)
+# define OSQP_PRIMAL_INFEASIBLE_INACCURATE (3)
+# define OSQP_SOLVED_INACCURATE (2)
+# define OSQP_SOLVED (1)
+# define OSQP_MAX_ITER_REACHED (-2)
+# define OSQP_PRIMAL_INFEASIBLE (-3)    /* primal infeasible  */
+# define OSQP_DUAL_INFEASIBLE (-4)      /* dual infeasible */
+# define OSQP_SIGINT (-5)               /* interrupted by user */
+# ifdef PROFILING
+#  define OSQP_TIME_LIMIT_REACHED (-6)
+# endif // ifdef PROFILING
+# define OSQP_NON_CVX (-7)              /* problem non convex */
+# define OSQP_UNSOLVED (-10)            /* Unsolved. Only setup function has been called */
+
+
+/*************************
+* Linear System Solvers *
+*************************/
+enum linsys_solver_type { QDLDL_SOLVER, MKL_PARDISO_SOLVER };
+extern const char * LINSYS_SOLVER_NAME[];
+
+
+/******************
+* Solver Errors  *
+******************/
+enum osqp_error_type {
+    OSQP_DATA_VALIDATION_ERROR = 1,  /* Start errors from 1 */
+    OSQP_SETTINGS_VALIDATION_ERROR,
+    OSQP_LINSYS_SOLVER_LOAD_ERROR,
+    OSQP_LINSYS_SOLVER_INIT_ERROR,
+    OSQP_NONCVX_ERROR,
+    OSQP_MEM_ALLOC_ERROR,
+    OSQP_WORKSPACE_NOT_INIT_ERROR,
+};
+extern const char * OSQP_ERROR_MESSAGE[];
+
+
+/**********************************
+* Solver Parameters and Settings *
+**********************************/
+
+# define RHO (0.1f)
+# define SIGMA (1E-06f)
+# define MAX_ITER (4000)
+# define EPS_ABS (1E-3f)
+# define EPS_REL (1E-3f)
+# define EPS_PRIM_INF (1E-4f)
+# define EPS_DUAL_INF (1E-4f)
+# define ALPHA (1.6f)
+# define LINSYS_SOLVER (QDLDL_SOLVER)
+
+# define RHO_MIN (1e-06f)
+# define RHO_MAX (1e06f)
+# define RHO_EQ_OVER_RHO_INEQ (1e03f)
+# define RHO_TOL (1e-04f) ///< tolerance for detecting if an inequality is set to equality
+
+
+# ifndef EMBEDDED
+#  define DELTA (1E-6)
+#  define POLISH (0)
+#  define POLISH_REFINE_ITER (3)
+#  define VERBOSE (1)
+# endif // ifndef EMBEDDED
+
+# define SCALED_TERMINATION (0)
+# define CHECK_TERMINATION (25)
+# define WARM_START (1)
+# define SCALING (10)
+
+# define MIN_SCALING (1e-04f) ///< minimum scaling value
+# define MAX_SCALING (1e+04f) ///< maximum scaling value
+
+
+# ifndef OSQP_NULL
+#  define OSQP_NULL 0
+# endif /* ifndef OSQP_NULL */
+
+# ifndef OSQP_NAN
+#  define OSQP_NAN ((c_float)0x7fc00000UL)  // not a number
+# endif /* ifndef OSQP_NAN */
+
+# ifndef OSQP_INFTY
+#  define OSQP_INFTY ((c_float)1e30)        // infinity
+# endif /* ifndef OSQP_INFTY */
+
+# ifndef OSQP_DIVISION_TOL
+#  define OSQP_DIVISION_TOL ((c_float)1.0 / OSQP_INFTY)
+# endif /* ifndef OSQP_DIVISION_TOL */
+
+
+# if EMBEDDED != 1
+#  define ADAPTIVE_RHO (1)
+#  define ADAPTIVE_RHO_INTERVAL (0)
+#  define ADAPTIVE_RHO_FRACTION (0.4)           ///< fraction of setup time after which we update rho
+#  define ADAPTIVE_RHO_MULTIPLE_TERMINATION (4) ///< multiple of check_termination after which we update rho (if PROFILING disabled)
+#  define ADAPTIVE_RHO_FIXED (100)              ///< number of iterations after which we update rho if termination_check  and PROFILING are disabled
+#  define ADAPTIVE_RHO_TOLERANCE (5)            ///< tolerance for adopting new rho; minimum ratio between new rho and the current one
+# endif // if EMBEDDED != 1
+
+# ifdef PROFILING
+#  define TIME_LIMIT (0)                        ///< Disable time limit as default
+# endif // ifdef PROFILING
+
+/* Printing */
+# define PRINT_INTERVAL 200
+
+
+# ifdef __cplusplus
+}
+# endif // ifdef __cplusplus
+
+#endif // ifndef CONSTANTS_H