Commit c56441de authored by bucyril's avatar bucyril
Browse files

corrected circle controller, made crazyflie crash safely

parent 6b6ece5f
...@@ -216,6 +216,7 @@ add_executable(PPSClient src/PPSClient.cpp) ...@@ -216,6 +216,7 @@ add_executable(PPSClient src/PPSClient.cpp)
add_executable(SafeControllerService src/SafeControllerService.cpp) add_executable(SafeControllerService src/SafeControllerService.cpp)
add_executable(CustomControllerService src/CustomControllerService.cpp) add_executable(CustomControllerService src/CustomControllerService.cpp)
add_executable(CentralManagerService src/CentralManagerService.cpp src/CrazyflieIO.cpp) add_executable(CentralManagerService src/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(CircleControllerService src/CircleControllerService.cpp)
...@@ -248,6 +249,7 @@ add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TA ...@@ -248,6 +249,7 @@ add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TA
add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CustomControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) add_dependencies(CustomControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) add_dependencies(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CircleControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
# GUI-- dependencies # GUI-- dependencies
...@@ -281,6 +283,8 @@ target_link_libraries(CustomControllerService ${catkin_LIBRARIES}) ...@@ -281,6 +283,8 @@ target_link_libraries(CustomControllerService ${catkin_LIBRARIES})
target_link_libraries(CentralManagerService ${catkin_LIBRARIES}) target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
target_link_libraries(CircleControllerService ${catkin_LIBRARIES})
# GUI -- link libraries # GUI -- link libraries
target_link_libraries(my_GUI Qt5::Widgets) # GUI -- let my_GUI have acesss to Qt stuff target_link_libraries(my_GUI Qt5::Widgets) # GUI -- let my_GUI have acesss to Qt stuff
......
...@@ -15,4 +15,4 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback ...@@ -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 estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
#setpoint in meters (x, y, z, yaw) #setpoint in meters (x, y, z, yaw)
defaultSetpoint: [-0.5, 0.0, 0.4, 1.2] defaultSetpoint: [0.0, 0.0, 0.0, 0.0]
...@@ -51,9 +51,9 @@ float saturationThrust; ...@@ -51,9 +51,9 @@ float saturationThrust;
CrazyflieData previousLocation; CrazyflieData previousLocation;
//circle stuff //circle stuff
float time; float currentTime;
const float OMEGA = 0.1*2*PI; const float OMEGA = 0.5*2*PI;
const float RADIUS = 0.2; const float RADIUS = 0.35;
void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) { void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
...@@ -141,17 +141,17 @@ void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) { ...@@ -141,17 +141,17 @@ void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
} }
void calculateCircle(Setpoint &circlePoint){ void calculateCircle(Setpoint &circlePoint){
circlePoint.x = RADIUS*cos(OMEGA*time); circlePoint.x = RADIUS*cos(OMEGA*currentTime);
circlePoint.y = RADIUS*sin(OMEGA*time); circlePoint.y = RADIUS*sin(OMEGA*currentTime);
circlePoint.z = 0.5; circlePoint.z = 0.5;
circlePoint.yaw = OMEGA*time; circlePoint.yaw = OMEGA*currentTime;
} }
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) { bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
CrazyflieData vicon = request.ownCrazyflie; CrazyflieData vicon = request.ownCrazyflie;
time += request.ownCrazyflie.acquiringTime; currentTime += request.ownCrazyflie.acquiringTime;
Setpoint circlePoint; Setpoint circlePoint;
calculateCircle(circlePoint); calculateCircle(circlePoint);
...@@ -212,7 +212,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -212,7 +212,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
ros::init(argc, argv, "CircleControllerService"); ros::init(argc, argv, "CircleControllerService");
time = 0; currentTime = 0;
ros::NodeHandle nodeHandle("~"); ros::NodeHandle nodeHandle("~");
loadParameters(nodeHandle); loadParameters(nodeHandle);
......
...@@ -125,7 +125,7 @@ void viconCallback(const ViconData& viconData) { ...@@ -125,7 +125,7 @@ void viconCallback(const ViconData& viconData) {
ROS_ERROR_STREAM("custom controller name: valid: " << customController.getService()); ROS_ERROR_STREAM("custom controller name: valid: " << customController.getService());
usingSafeController = true; usingSafeController = true;
} else { } else {
usingSafeController = safetyCheck(global, controllerCall.response.controlOutput); usingSafeController = !safetyCheck(global, controllerCall.response.controlOutput);
if(usingSafeController) { if(usingSafeController) {
ROS_INFO_STREAM("safety check failed, switching to safe controller"); ROS_INFO_STREAM("safety check failed, switching to safe controller");
} }
......
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