resolve crashing on flash to pixracer pro

ntnu-arl · Feb 7, 2025 · 30f44cb · 30f44cb
1 parent b502ad9
commit 30f44cb
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 19 deletions.
diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt
@@ -39,6 +39,7 @@ px4_add_module(
 	MODULE modules__mc_pos_control
 	MAIN mc_pos_control
 	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		MulticopterPositionControl.cpp
 		MulticopterPositionControl.hpp
@@ -49,5 +50,6 @@ px4_add_module(
 		controllib
 		geo
 		SlewRate
+		mathlib
 		qpoases
 	)
diff --git a/src/modules/mc_pos_control/PositionControl/CBFSafetyFilter.cpp b/src/modules/mc_pos_control/PositionControl/CBFSafetyFilter.cpp
@@ -3,15 +3,6 @@
  */
 #include <CBFSafetyFilter.hpp>
 
-#include <px4_platform_common/module.h>
-#include <math.h>
-#include <string.h>
-
-CBFSafetyFilter::CBFSafetyFilter() {
-    qp = QProblem(NV, NC);
-    qp.setPrintLevel(PL_NONE);
-}
-
 void CBFSafetyFilter::updateObstacles() {
     tof_obstacles_chunk_s tof_obstacles_chunk;
     if (_tof_obstacles_chunk_sub.update(&tof_obstacles_chunk))
@@ -177,14 +168,15 @@ void CBFSafetyFilter::filter(Vector3f& acceleration_setpoint, const Vector3f& ve
     real_t* ub = NULL;
     int_t nWSR = 50;
 
-    qp = QProblem(NV, NC);
+    QProblem qp(NV, NC);
     qp.setPrintLevel(PL_NONE);
     returnValue qp_status = qp.init(H, g, A, lb, ub, lbA, ubA, nWSR);
 
+    real_t xOpt[NV];
     switch(qp_status) {
         case SUCCESSFUL_RETURN: {
-            qp.getPrimalSolution(_xOpt);
-            Vector3f acceleration_correction(_xOpt[0], _xOpt[1], _xOpt[2]);
+            qp.getPrimalSolution(xOpt);
+            Vector3f acceleration_correction(xOpt[0], xOpt[1], xOpt[2]);
             _body_acceleration_setpoint += acceleration_correction;
             acceleration_setpoint = R_WB * _body_acceleration_setpoint;
             _debug_msg.qp_fail = 0;
@@ -207,8 +199,8 @@ void CBFSafetyFilter::filter(Vector3f& acceleration_setpoint, const Vector3f& ve
     _debug_msg.output[0] = acceleration_setpoint(0);
     _debug_msg.output[1] = acceleration_setpoint(1);
     _debug_msg.output[2] = acceleration_setpoint(2);
-    _debug_msg.slack[0] = _xOpt[3];
-    _debug_msg.slack[1] = _xOpt[4];
+    _debug_msg.slack[0] = xOpt[3];
+    _debug_msg.slack[1] = xOpt[4];
 }
 
 void CBFSafetyFilter::clampAccSetpoint(Vector3f& acceleration_setpoint) {

diff --git a/src/modules/mc_pos_control/PositionControl/CBFSafetyFilter.hpp b/src/modules/mc_pos_control/PositionControl/CBFSafetyFilter.hpp
@@ -23,7 +23,7 @@ using namespace qpOASES;
 class CBFSafetyFilter
 {
 public:
-    CBFSafetyFilter();
+    CBFSafetyFilter() = default;
 	~CBFSafetyFilter() = default;
 
     void updateObstacles();
@@ -64,7 +64,7 @@ class CBFSafetyFilter
     px4::Array<Vector3f, CBF_MAX_OBSTACLES> _obstacles;
     px4::Array<float, CBF_MAX_OBSTACLES> _nu1;
 
-    cbf_debug_s _debug_msg;
+    cbf_debug_s _debug_msg{};
 
     float _epsilon;
     float _pole0;
@@ -83,7 +83,4 @@ class CBFSafetyFilter
     float saturate(float x);
     float saturateDerivative(float x);
     float kappaFunction(float h, float alpha);
-
-    QProblem qp;
-    real_t _xOpt[NV];
 };