cherry pick 39c1f7

from https://github.com/ntnu-arl/modalai-px4-firmware
ntnu-arl · Feb 7, 2025 · b502ad9 · b502ad9
1 parent 0c6027d
commit b502ad9
Show file tree

Hide file tree

Showing 5 changed files with 9 additions and 7 deletions.
diff --git a/msg/CbfDebug.msg b/msg/CbfDebug.msg
@@ -1,5 +1,6 @@
 uint64 timestamp          # time since system start (microseconds)
 
+uint32 cbf_duration
 uint8 qp_fail
 
 float32 h

diff --git a/src/modules/mavlink/streams/CBF_DEBUG.hpp b/src/modules/mavlink/streams/CBF_DEBUG.hpp
@@ -68,6 +68,7 @@ class MavlinkStreamCbfDebug : public MavlinkStream
 
 			msg.time_usec = cbf_debug.timestamp;
 
+			msg.cbf_duration = cbf_debug.cbf_duration;
 			msg.qp_fail = cbf_debug.qp_fail;
 			msg.h = cbf_debug.h;
 			msg.h1 = cbf_debug.h1;

diff --git a/src/modules/mc_pos_control/PositionControl/CBFSafetyFilter.cpp b/src/modules/mc_pos_control/PositionControl/CBFSafetyFilter.cpp
@@ -61,8 +61,9 @@ void CBFSafetyFilter::updateAttitude() {
     }
 }
 
-void CBFSafetyFilter::filter(Vector3f& acceleration_setpoint, const Vector3f& velocity, uint64_t timestamp) {
+void CBFSafetyFilter::filter(Vector3f& acceleration_setpoint, const Vector3f& velocity) {
     if  (!_enabled) return;
+    uint64_t tic = hrt_absolute_time();
 
     // TODO reset obstacle with timer
     // pass through if no obstacles are recorded
@@ -201,6 +202,8 @@ void CBFSafetyFilter::filter(Vector3f& acceleration_setpoint, const Vector3f& ve
 
     clampAccSetpoint(acceleration_setpoint);
 
+    uint64_t toc = hrt_absolute_time();
+    _debug_msg.cbf_duration = (uint32_t)(toc-tic);
     _debug_msg.output[0] = acceleration_setpoint(0);
     _debug_msg.output[1] = acceleration_setpoint(1);
     _debug_msg.output[2] = acceleration_setpoint(2);

diff --git a/src/modules/mc_pos_control/PositionControl/CBFSafetyFilter.hpp b/src/modules/mc_pos_control/PositionControl/CBFSafetyFilter.hpp
@@ -29,7 +29,7 @@ class CBFSafetyFilter
     void updateObstacles();
     void updateAttitude();
 
-    void filter(Vector3f& acceleration_setpoint, const Vector3f& velocity, uint64_t timestamp);
+    void filter(Vector3f& acceleration_setpoint, const Vector3f& velocity);
 
     void setEpsilon(float epsilon) { _epsilon = epsilon; }
     void setPole0(float pole0) { _pole0 = pole0; }
@@ -42,6 +42,7 @@ class CBFSafetyFilter
 
     void getDebug(cbf_debug_s& debug_msg)
     {
+        debug_msg.cbf_duration = _debug_msg.cbf_duration;
         debug_msg.qp_fail = _debug_msg.qp_fail;
         debug_msg.h = _debug_msg.h;
         debug_msg.h1 = _debug_msg.h1;

diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
@@ -149,12 +149,8 @@ void PositionControl::_velocityControl(const float dt)
 	// No control input from setpoints or corresponding states which are NAN
 	ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);
 
-	// TODO: add enable flag as parameter
 	// TODO: check if _vel can be NAN
-	// [[maybe_unused]] hrt_abstime tic = hrt_absolute_time();
-	_cbf.filter(_acc_sp, _vel, hrt_absolute_time());
-	// [[maybe_unused]] hrt_abstime toc = hrt_absolute_time();
-	// PX4_INFO("cbf_filter duration: %llu", toc - tic);
+	_cbf.filter(_acc_sp, _vel);
 
 	_accelerationControl();