Commit b52b80e2 authored by Paul Beuchat's avatar Paul Beuchat
Browse files

now the take-offis also w.r.t. the loads yaw angle

parent 24c80da3
......@@ -184,7 +184,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
load_setpoint[0] = request.otherCrazyflies[3].x;
load_setpoint[1] = request.otherCrazyflies[3].y;
load_setpoint[2] = 0.0;
float yaw = request.otherCrazyflies[0].yaw;
float yaw = request.otherCrazyflies[3].yaw;
while(yaw > PI) {yaw -= 2 * PI;}
while(yaw < -PI) {yaw += 2 * PI;}
load_setpoint[3] = yaw;
......@@ -327,7 +327,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
load_setpoint[0] = request.otherCrazyflies[3].x;
load_setpoint[1] = request.otherCrazyflies[3].y;
load_setpoint[2] = request.otherCrazyflies[3].z;
float yaw = request.otherCrazyflies[0].yaw;
float yaw = request.otherCrazyflies[3].yaw;
while(yaw > PI) {yaw -= 2 * PI;}
while(yaw < -PI) {yaw += 2 * PI;}
load_setpoint[3] = yaw;
......@@ -406,7 +406,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// load z-setpoint is current load z-position
load_setpoint[0] = request.otherCrazyflies[3].x;
load_setpoint[1] = request.otherCrazyflies[3].y;
load_setpoint[2] = request.otherCrazyflies[3].z;
load_setpoint[2] = request.otherCrazyflies[3].z;
load_setpoint[3] = request.otherCrazyflies[3].yaw;
}
// Distributed Controller runs at 100Hz --> downsampling by factor 2
......@@ -681,14 +682,14 @@ std::vector<float> transformation_frame_load_to_world(std::vector<float> P_load
float P_y_world = 0.0;
P_y_world += sin_yaw_load*cos_pitch_load*P_load[0];
P_y_world += ( cos_yaw_load*cos_roll_load + sin_yaw_load*sin_pitch_load*sin_roll_load ) * P_load[1];
P_y_world += ( -cos_yaw_load*sin_yaw_load + sin_yaw_load*sin_pitch_load*cos_roll_load ) * P_load[2];
P_y_world += ( -cos_yaw_load*sin_roll_load + sin_yaw_load*sin_pitch_load*cos_roll_load ) * P_load[2];
// zP
float P_z_world = 0.0;
P_z_world += -sin_pitch_load * P_load[0];
P_z_world += cos_pitch_load * sin_roll_load * P_load[1];
P_z_world += cos_pitch_load * cos_roll_load * P_load[2];
std::vector<float> P_world = {0 ,0 ,0};
std::vector<float> P_world = {0.0 , 0.0 , 0.0 };
P_world[0] = P_x_world;
P_world[1] = P_y_world;
P_world[2] = P_z_world;
......@@ -1501,7 +1502,7 @@ int main(int argc, char* argv[]) {
centralized_controller.Phi = readMatrix( filename_Phi.append("Phi.txt" ) );
outputMatrix_centralized = readMatrix( filename_C.append( "C.txt" ) );
//ROS_INFO_STREAM("[STUDENT CONTROLLER] outputMatrix = " << outputMatrix);
ROS_INFO_STREAM("[STUDENT CONTROLLER] outputMatrix = " << outputMatrix);
// Initialize some state vector of the output feedback 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