From a98ba8bea04751c047319e8314db9e750f00c66a Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Mon, 9 Apr 2018 15:18:20 +0200
Subject: [PATCH] local coordinates box fix
---
.../GUI_Qt/studentGUI/src/MainWindow.cpp | 40 +++++++++++--------
1 file changed, 24 insertions(+), 16 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
index 18d47630..0f4625d4 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
@@ -711,35 +711,43 @@ Setpoint MainWindow::correctSetpointBox(Setpoint setpoint, CrazyflieContext cont
Setpoint corrected_setpoint;
corrected_setpoint = setpoint;
- if(setpoint.x > context.localArea.xmax)
- corrected_setpoint.x = context.localArea.xmax;
- if(setpoint.y > context.localArea.ymax)
- corrected_setpoint.y = context.localArea.ymax;
- if(setpoint.z > context.localArea.zmax)
- corrected_setpoint.z = context.localArea.zmax;
-
- if(setpoint.x < context.localArea.xmin)
- corrected_setpoint.x = context.localArea.xmin;
- if(setpoint.y < context.localArea.ymin)
- corrected_setpoint.y = context.localArea.ymin;
- if(setpoint.z < context.localArea.zmin)
- corrected_setpoint.z = context.localArea.zmin;
+ float x_size = context.localArea.xmax - context.localArea.xmin;
+ float y_size = context.localArea.xmax - context.localArea.xmin;
+ float z_size = context.localArea.xmax - context.localArea.xmin;
+
+ if(setpoint.x > x_size/2)
+ corrected_setpoint.x = x_size/2;
+ if(setpoint.y > y_size/2)
+ corrected_setpoint.y = y_size/2;
+ if(setpoint.z > z_size)
+ corrected_setpoint.z = z_size;
+
+ if(setpoint.x < -x_size/2)
+ corrected_setpoint.x = -x_size/2;
+ if(setpoint.y < -y_size/2)
+ corrected_setpoint.y = -y_size/2;
+ if(setpoint.z < 0)
+ corrected_setpoint.z = 0;
return corrected_setpoint;
}
bool MainWindow::setpointInsideBox(Setpoint setpoint, CrazyflieContext context)
{
+
+ float x_size = context.localArea.xmax - context.localArea.xmin;
+ float y_size = context.localArea.xmax - context.localArea.xmin;
+ float z_size = context.localArea.xmax - context.localArea.xmin;
//position check
- if((setpoint.x < context.localArea.xmin) or (setpoint.x > context.localArea.xmax)) {
+ if((setpoint.x < -x_size/2) or (setpoint.x > x_size/2)) {
ROS_INFO_STREAM("x outside safety box");
return false;
}
- if((setpoint.y < context.localArea.ymin) or (setpoint.y > context.localArea.ymax)) {
+ if((setpoint.y < -y_size/2) or (setpoint.y > y_size/2)) {
ROS_INFO_STREAM("y outside safety box");
return false;
}
- if((setpoint.z < context.localArea.zmin) or (setpoint.z > context.localArea.zmax)) {
+ if((setpoint.z < 0) or (setpoint.z > z_size)) {
ROS_INFO_STREAM("z outside safety box");
return false;
}
--
GitLab