Commit a6c20a60 authored by phfriedl's avatar phfriedl
Browse files

added debugging message for easier creation of .bag files

parent 939d2e9b
......@@ -112,6 +112,8 @@ add_message_files(
Setpoint.msg
CrazyflieEntry.msg
CrazyflieDB.msg
#----------------------------------------------------------------------
Debugging.msg
)
## Generate services in the 'srv' folder
......
......@@ -15,5 +15,5 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
#setpoint in meters (x, y, z, yaw)
defaultSetpoint: [0.0, 0.0, 0.1, 0.0]
defaultSetpoint: [-0.7, -0.1, 0.25, 0.0]
......@@ -26,6 +26,7 @@
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/Debugging.h" //---------------------------------------------------------------------------
#define PI 3.1415926535
#define RATE_CONTROLLER 0
......@@ -169,7 +170,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//bag.write("ViconData", ros::Time::now(), request.ownCrazyflie);
//trial>>>>>>>
int yaw_measured = request.ownCrazyflie.yaw;
float yaw_measured = request.ownCrazyflie.yaw;
//<<<<<<
//move coordinate system to make setpoint origin
......@@ -180,21 +181,48 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//bag.write("Offset", ros::Time::now(), request.ownCrazyflie);
while(yaw > PI) yaw -= 2 * PI;
while(yaw < -PI) yaw += 2 * PI;
while(yaw > PI) {yaw -= 2 * PI;}
while(yaw < -PI) {yaw += 2 * PI;}
request.ownCrazyflie.yaw = yaw;
float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
estimateState(request, est);
std_msgs::Float32 EstXTest;
EstXTest.data = est[0];
bag.write("EstimateX", ros::Time::now(), EstXTest);
//CONTROLLER DEBUGGING--------------------------------------------------------------------------------------------------
Debugging estTests;
estTests.x = est[0];
estTests.y = est[1];
estTests.z = est[2];
estTests.vx = est[3];
estTests.vy = est[4];
estTests.vz = est[5];
estTests.roll = est[6];
estTests.pitch = est[7];
estTests.yaw = est[8];
bag.write("Debugging est", ros::Time::now(), estTests);
//CONTROLLER DEBUGGING END----------------------------------------------------------------------------------------------
float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
convertIntoBodyFrame(est, state, yaw_measured);
//convertIntoBodyFrame(est, state, yaw);
//CONTROLLER DEBUGGING--------------------------------------------------------------------------------------------------
estTests.x = state[0];
estTests.y = state[1];
estTests.z = state[2];
estTests.vx = state[3];
estTests.vy = state[4];
estTests.vz = state[5];
estTests.roll = state[6];
estTests.pitch = state[7];
estTests.yaw = state[8];
bag.write("Debugging state", ros::Time::now(), estTests);
std_msgs::Float32 f32;
f32.data = yaw_measured;
bag.write("yaw measured", ros::Time::now(), f32);
//CONTROLLER DEBUGGING END----------------------------------------------------------------------------------------------
//calculate feedback
float outRoll = 0;
......
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