From 612dfe49e04b24ef6432020285b3161e9e35be4d Mon Sep 17 00:00:00 2001
From: Angel <roangel@student.ethz.ch>
Date: Tue, 26 Sep 2017 12:08:35 +0200
Subject: [PATCH] tried to change way of changing controller. Need to test
---
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 39 ++++++++++++++++---------
1 file changed, 26 insertions(+), 13 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 7d996ad0..58758a43 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -120,7 +120,10 @@ int crazyradio_status;
CrazyflieContext context;
//wheter to use safe of custom controller
-bool using_safe_controller;
+int instant_controller; //variable for the instant controller, e.g., we use safe controller for taking off and landing even if custom controller is enabled. This variable WILL change automatically
+
+// controller used:
+int controller_used = SAFE_CONTROLLER;
ros::Publisher controllerUsedPublisher;
@@ -138,27 +141,34 @@ ros::Timer timer_takeoff;
ros::Timer timer_land;
+
+void setControllerUsed(int controller)
+{
+ controller_used = controller;
+}
+
+int getControllerUsed()
+{
+ return controller_used;
+}
+
void useSafeController()
{
- using_safe_controller = true;
- // send a message in topic for the studentGUI to read it
- std_msgs::Int32 controller_used_msg;
- controller_used_msg.data = SAFE_CONTROLLER;
- controllerUsedPublisher.publish(controller_used_msg);
+ setControllerUsed(SAFE_CONTROLLER);
+
}
-void useCustomController()
+void sendMessageUsingController(int controller)
{
- using_safe_controller = false;
// send a message in topic for the studentGUI to read it
std_msgs::Int32 controller_used_msg;
- controller_used_msg.data = CUSTOM_CONTROLLER;
+ controller_used_msg.data = controller;
controllerUsedPublisher.publish(controller_used_msg);
}
-bool getUsingSafeController()
+void useCustomController()
{
- return using_safe_controller;
+ setControllerUsed(CUSTOM_CONTROLLER);
}
void loadSafeController() {
@@ -401,12 +411,14 @@ void viconCallback(const ViconData& viconData) {
break;
}
+ sendMessageUsingController(getControllerUsed());
+
// control part that should always be running, calls to controller, computation of control output
if(flying_state != STATE_MOTORS_OFF)
{
if(!global.occluded) //if it is not occluded, then proceed to compute the controller output.
{
- if(!getUsingSafeController() && flying_state == STATE_FLYING) // only enter in custom controller if we are not using safe controller AND the flying state is FLYING
+ if(getControllerUsed() == CUSTOM_CONTROLLER && flying_state == STATE_FLYING) // only enter in custom controller if we are not using safe controller AND the flying state is FLYING
{
bool success = customController.call(controllerCall);
if(!success)
@@ -422,7 +434,7 @@ void viconCallback(const ViconData& viconData) {
ROS_INFO_STREAM("safety check failed, switching to safe controller");
}
}
- else
+ else //SAFE_CONTROLLER and state is different from flying
{
calculateDistanceToCurrentSafeSetpoint(local); // update distance, it also updates the unit vector
ROS_INFO_STREAM("distance: " << distance);
@@ -717,6 +729,7 @@ int main(int argc, char* argv[])
flying_state = STATE_MOTORS_OFF;
useSafeController();
loadSafeController();
+ sendMessageUsingController(getControllerUsed());
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
--
GitLab