Commit a6ca8ca9 authored by muelmarc's avatar muelmarc
Browse files

Small Changes on safecontroller

parent f8101372
......@@ -116,7 +116,7 @@ if __name__ == '__main__':
if rospy.has_param("/CrazyRadio/CrazyFlieAddress"):
radio_address = rospy.get_param("/CrazyRadio/CrazyFlieAddress")
rospy.loginfo("Connecting to %s" % radio_address)
rospy.loginfo("Crazyradio connecting to %s" % radio_address)
global cf_client
#TODO: load address from parameters
......
......@@ -24,6 +24,7 @@
#include "ros/ros.h"
#include <stdlib.h>
//include autogenerated headers from srv files
#include "d_fall_pps/RateController.h"
......@@ -93,14 +94,15 @@ ControllerOutput ControlCommandTest;
//not void: sould give back controlldata
void ppsClientToController(ViconData data, bool autocontrolOn){
if(data.crazyflieName == cflie){
//call safecontroller if autocontrol is true
if(autocontrolOn){
//call safecontroller if autocontrol is true
ControlCommandTest.motorCmd1 = 0;
ControlCommandTest.motorCmd2 = 0;
ControlCommandTest.motorCmd3 = 0;
ControlCommandTest.motorCmd4 = 0;
}
else {
//
ControlCommandTest.motorCmd1 = 1000;
ControlCommandTest.motorCmd2 = 1000;
ControlCommandTest.motorCmd3 = 1000;
......@@ -143,6 +145,15 @@ bool safetyCheck(ViconData data){
//ROS_INFO_STREAM("ViconData: " << data.x << ", " << data.y << ", " << data.z);
//position check
if((data.x < Area.areaBoundxmin) or (data.x < Area.areaBoundxmax)){
return true;
}
if((data.y < Area.areaBoundymin) or (data.z < Area.areaBoundymax)){
return true;
}
if((data.z < Area.areaBoundzmin) or (data.z < Area.areaBoundzmax)){
return true;
}
//attitude check
......
......@@ -36,10 +36,10 @@ bool calculateControlOutput(RateController::Request &request, RateController::Re
ViconData vicon = request.crazyflieLocation;
Setpoint goal = request.setpoint;
ROS_INFO("request received with following ViconData");
ROS_INFO_STREAM(vicon);
ROS_INFO("the goal setpoint is:");
ROS_INFO_STREAM(request.setpoint);
//ROS_INFO("request received with following ViconData");
//ROS_INFO_STREAM(vicon);
//ROS_INFO("the goal setpoint is:");
//ROS_INFO_STREAM(request.setpoint);
//add/calculate safeController
response.controlOutput.rollRate = 1; //testvalue
......
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