diff --git a/pps_wiki/ros_structure.md b/pps_wiki/ros_structure.md
index a439eaddd1f7f2a3150a85a1f73997079ce99980..76cdec8aba9c43898926812eb40b14b981a51da9 100644
--- a/pps_wiki/ros_structure.md
+++ b/pps_wiki/ros_structure.md
@@ -39,6 +39,8 @@ The ``PPSClient`` subscribes to a local topic with which a certain controller ca
In file ``ClientConfig.yaml``:
- ``safeController``: the relative ros path to the safe controller
- ``customController``: the relative ros path to the custom controller
+- ``strictSafety``: turns angle safety check on and off
+- ``angleMargin``: defines maximal allowed angle when strict safety is on, 1 means 90 degrees
### CrazyRadio
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp
index b4c039c0425ffeb422c3355924615db4811039c2..49e4b00e487b0bc020e76b80222dad7719e5f40e 100755
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp
@@ -739,6 +739,9 @@ void MainGUIWindow::on_save_in_DB_button_clicked()
tmp_entry.crazyflieContext.localArea.xmax = x_max * FROM_UNITS_TO_METERS;
tmp_entry.crazyflieContext.localArea.ymin = y_min * FROM_UNITS_TO_METERS;
tmp_entry.crazyflieContext.localArea.ymax = y_max * FROM_UNITS_TO_METERS;
+
+ tmp_entry.crazyflieContext.localArea.zmin = -0.2;
+ tmp_entry.crazyflieContext.localArea.zmax = 2.0;
}
}
tmp_db.crazyflieEntries.push_back(tmp_entry);
diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
index 8c81ff60e4f6fa18ec3bdc5c78b2a043df46e58f..27a48ab3df067ca6aba1f6f0a79beaae689817b3 100755
--- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
+++ b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
@@ -1,3 +1,5 @@
safeController: "SafeControllerService/RateController"
-customController: "SafeControllerService/RateController"
+customController: "CircleControllerService/CircleController"
+strictSafety: true
+angleMargin: 0.6
diff --git a/pps_ws/src/d_fall_pps/param/SafeController.yaml b/pps_ws/src/d_fall_pps/param/SafeController.yaml
index a2f9b9639fffa58f324b5157216b6d91fa484844..1997d4dac71a04513b72ef1db0913c5ff4f2196d 100644
--- a/pps_ws/src/d_fall_pps/param/SafeController.yaml
+++ b/pps_ws/src/d_fall_pps/param/SafeController.yaml
@@ -15,4 +15,4 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
#setpoint in meters (x, y, z, yaw)
-defaultSetpoint: [0.0, 0.0, 0.0, 0.0]
+defaultSetpoint: [0.0, 0.0, 0.3, 0.0]
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 53977984ddd5306ee35315b386541c682486ba13..225e4d5d1ec00322dfa5c0a81a6af9614f90b5bd 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -37,15 +37,20 @@
#define CMD_USE_CRAZYFLY_ENABLE 3
#define CMD_USE_CRAZYFLY_DISABLE 4
+#define PI 3.141592653589
+
using namespace d_fall_pps;
//studentID, gives namespace and identifier in CentralManagerService
int studentID;
-//the safe controller specified in the ClientConfig.yaml, is considered trusted
+//the safe controller specified in the ClientConfig.yaml, is considered stable
ros::ServiceClient safeController;
-//the custom controller specified in the ClientConfig.yaml, is considered untrusted
+//the custom controller specified in the ClientConfig.yaml, is considered potentially unstable
ros::ServiceClient customController;
+//values for safteyCheck
+bool strictSafety;
+float angleMargin;
ros::ServiceClient centralManager;
ros::Publisher controlCommandPublisher;
@@ -55,37 +60,40 @@ rosbag::Bag bag;
//describes the area of the crazyflie and other parameters
CrazyflieContext context;
-//gather information about other crazyflies --------------------------------------------------------------------------------
-/*bool getOtherCrazyflies;
-bool getAllCrazyflies;
-std::vector<Setpoint> otherSetpoints;
-*/
-//------------------------------------------------------------------------------------
-
//wheter to use safe of custom controller
bool usingSafeController;
//wheter crazyflie is enabled (ready to fly) or disabled (motors off)
bool crazyflieEnabled;
-int safetyDelay;
-
//checks if crazyflie is within allowed area and if custom controller returns no data
bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
-
//position check
if((data.x < context.localArea.xmin) or (data.x > context.localArea.xmax)) {
- safetyDelay--;
+ ROS_INFO_STREAM("x safety failed");
return false;
}
if((data.y < context.localArea.ymin) or (data.y > context.localArea.ymax)) {
- safetyDelay--;
+ ROS_INFO_STREAM("y safety failed");
return false;
}
if((data.z < context.localArea.zmin) or (data.z > context.localArea.zmax)) {
- safetyDelay--;
+ ROS_INFO_STREAM("z safety failed");
return false;
}
+ //attitude check
+ //if strictSafety is set to true in ClientConfig.yaml the SafeController takes also over if the roll and pitch angles get to large
+ //the angleMargin is a value in the range (0,1). The closer to 1, the closer to 90 deg are the roll and pitch angles allowed to become before the safeController takes over
+ if(strictSafety){
+ if((data.roll > PI/2*angleMargin) or (data.roll < -PI/2*angleMargin)) {
+ ROS_INFO_STREAM("roll too big.");
+ return false;
+ }
+ if((data.pitch > PI/2*angleMargin) or (data.pitch < -PI/2*angleMargin)) {
+ ROS_INFO_STREAM("pitch too big.");
+ return false;
+ }
+ }
return true;
}
@@ -158,6 +166,16 @@ void loadParameters(ros::NodeHandle& nodeHandle) {
if(!nodeHandle.getParam("studentID", studentID)) {
ROS_ERROR("Failed to get studentID");
}
+ if(!nodeHandle.getParam("strictSafety", strictSafety)) {
+ ROS_ERROR("Failed to get strictSafety param");
+ return;
+ }
+ if(!nodeHandle.getParam("angleMargin", angleMargin)) {
+ ROS_ERROR("Failed to get angleMargin param");
+ return;
+ }
+
+
}
void loadCrazyflieContext() {