Commit 4ebd0bcc authored by phfriedl's avatar phfriedl
Browse files

changes to CMakeList, Crazyflie.launch and CustomControllerService -- all...

changes to CMakeList, Crazyflie.launch and CustomControllerService -- all concerning the CustomControllerService
parent 75ecf43e
...@@ -189,6 +189,7 @@ include_directories( ...@@ -189,6 +189,7 @@ include_directories(
add_executable(ViconDataPublisher src/ViconDataPublisher.cpp) add_executable(ViconDataPublisher src/ViconDataPublisher.cpp)
add_executable(PPSClient src/PPSClient.cpp) 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(CentralManagerService src/CentralManagerService.cpp) add_executable(CentralManagerService src/CentralManagerService.cpp)
add_executable(my_GUI GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp GUI_Qt/CrazyFlyGUI/src/main.cpp ${UIS_HDRS} ${SRC_HDRS_QOBJECT}) add_executable(my_GUI GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp GUI_Qt/CrazyFlyGUI/src/main.cpp ${UIS_HDRS} ${SRC_HDRS_QOBJECT})
...@@ -208,6 +209,7 @@ set(MY_LIB_SOURCES # compilation of sources ...@@ -208,6 +209,7 @@ set(MY_LIB_SOURCES # compilation of sources
add_dependencies(ViconDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) add_dependencies(ViconDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
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(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) add_dependencies(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
## Rename C++ executable without prefix ## Rename C++ executable without prefix
...@@ -234,6 +236,8 @@ target_link_libraries(PPSClient ${catkin_LIBRARIES}) ...@@ -234,6 +236,8 @@ target_link_libraries(PPSClient ${catkin_LIBRARIES})
target_link_libraries(SafeControllerService ${catkin_LIBRARIES}) target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
target_link_libraries(CustomControllerService ${catkin_LIBRARIES})
target_link_libraries(CentralManagerService ${catkin_LIBRARIES}) target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
......
...@@ -20,6 +20,10 @@ ...@@ -20,6 +20,10 @@
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" /> <rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
</node> </node>
<node pkg="d_fall_pps" name="CustomControllerService" output="screen" type="CustomControllerService">
</node>
<node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" /> <node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" />
......
...@@ -19,11 +19,11 @@ ...@@ -19,11 +19,11 @@
#define ANGLE_MODE 1 #define ANGLE_MODE 1
#define MOTOR_MODE 2 #define MOTOR_MODE 2
//constats that you most probably need for your controller to work properly //constants that you most probably need for your controller to work properly
//see: 2015-08 - Forster - System ID of Crazyflie 2.pdf Chapter 3.3.1: Input Command → Thrust //see: 2015-08 - Forster - System ID of Crazyflie 2.pdf Chapter 3.3.1: Input Command → Thrust
const std::vector<float> FEEDFORWARD_MOTOR() = {37000, 37000, 37000, 37000}; const float FEEDFORWARD_MOTOR[4] = {37000, 37000, 37000, 37000};
const std::vector<float> MOTOR_REGRESSION_POLYNOMIAL() = {5.484560e-4, 1.032633e-6, 2.130295e-11}; const float MOTOR_REGRESSION_POLYNOMIAL[3] = {5.484560e-4, 1.032633e-6, 2.130295e-11};
const float SATURATION_THRUST = MOTOR_REGRESSION_POLYNOMIAL(2) * 12000 * 12000 + MOTOR_REGRESSION_POLYNOMIAL(1) * 12000 + MOTOR_REGRESSION_POLYNOMIAL(0); const float SATURATION_THRUST = MOTOR_REGRESSION_POLYNOMIAL[2] * 12000 * 12000 + MOTOR_REGRESSION_POLYNOMIAL[1] * 12000 + MOTOR_REGRESSION_POLYNOMIAL[1];
//namespacing the package //namespacing the package
using namespace d_fall_pps; using namespace d_fall_pps;
...@@ -38,17 +38,17 @@ void convertIntoBodyFrame(float est[9], float (&estBody)[9], int yaw_measured) { ...@@ -38,17 +38,17 @@ void convertIntoBodyFrame(float est[9], float (&estBody)[9], int yaw_measured) {
float sinYaw = sin(yaw_measured); float sinYaw = sin(yaw_measured);
float cosYaw = cos(yaw_measured); float cosYaw = cos(yaw_measured);
state[0] = est[0] * cosYaw + est[1] * sinYaw; estBody[0] = est[0] * cosYaw + est[1] * sinYaw;
state[1] = -est[0] * sinYaw + est[1] * cosYaw; estBody[1] = -est[0] * sinYaw + est[1] * cosYaw;
state[2] = est[2]; estBody[2] = est[2];
state[3] = est[3] * cosYaw + est[4] * sinYaw; estBody[3] = est[3] * cosYaw + est[4] * sinYaw;
state[4] = -est[3] * sinYaw + est[4] * cosYaw; estBody[4] = -est[3] * sinYaw + est[4] * cosYaw;
state[5] = est[5]; estBody[5] = est[5];
state[6] = est[6]; estBody[6] = est[6];
state[7] = est[7]; estBody[7] = est[7];
state[8] = est[8]; estBody[8] = est[8];
} }
......
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