MpcTracker: fxd bug with under-safety-area takeoff

ctu-mrs · Mar 6, 2024 · 67655a3 · 67655a3
1 parent 8b70172
commit 67655a3
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/src/mpc_tracker/mpc_tracker.cpp b/src/mpc_tracker/mpc_tracker.cpp
@@ -1790,14 +1790,15 @@ double MpcTracker::checkTrajectoryForCollisions(int& first_collision_index) {
     }
     u++;
   }
+
   if (!avoiding_collision_) {
 
     auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
 
     // we are not avoiding any collisions, so we slowly reduce the collision avoidance offset to return to normal flight
     collision_free_altitude_ -= 2.0 / (1.0 / dt1);
 
-    double safety_area_min_z = common_handlers_->safety_area.getMinZ("");
+    double safety_area_min_z = std::numeric_limits<double>::lowest();
 
     if (collision_free_altitude_ < safety_area_min_z) {
 
@@ -2059,7 +2060,7 @@ void MpcTracker::calculateMPC() {
 
   } else {
 
-    minimum_collison_free_altitude_ = common_handlers_->safety_area.getMinZ("");
+    minimum_collison_free_altitude_ = std::numeric_limits<double>::lowest();
   }
 
   double max_speed_x = constraints.horizontal_speed;