Commit 612dfe49 authored by roangel's avatar roangel
Browse files

tried to change way of changing controller. Need to test

parent 7227f8d0
......@@ -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") + "/";
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment