Commit 889e4871 authored by Tobias's avatar Tobias
Browse files

fixed a small error concerning y_eq

parent b2baa8ce
......@@ -10,7 +10,7 @@ motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
#take off and landing parameters (in meters and seconds)
takeOffinitial : 0.05
takeOffDistance : 0.6
takeOffDistance : 0.3
landingDistance : 0.1
durationTakeOff : 2.0
durationLanding : 2.0
......
......@@ -579,6 +579,13 @@ void get_y_from_CFData( Controller::Request &request )
std::vector<float> P3_world;
P3_world = transformation_frame_load_to_world( attachmentPoint.P3, roll , pitch , yaw );
std::vector<float> P1_world_eq;
P1_world_eq = transformation_frame_load_to_world( attachmentPoint.P1, 0.0 , 0.0 , load_setpoint[3] );
std::vector<float> P2_world_eq;
P2_world_eq = transformation_frame_load_to_world( attachmentPoint.P2, 0.0 , 0.0 , load_setpoint[3] );
std::vector<float> P3_world_eq;
P3_world_eq = transformation_frame_load_to_world( attachmentPoint.P3, 0.0 , 0.0 , load_setpoint[3] );
// y1 = [CF01.x , CF01.y , CF01.z, CF01.roll, CF01.pitch, CF01.yaw , len_cable*phi1 , len_cable*theta1 ]
yaw = request.otherCrazyflies[0].yaw;
while(yaw > PI) {yaw -= 2 * PI;}
......@@ -627,15 +634,15 @@ void get_y_from_CFData( Controller::Request &request )
//Eigen::Matrix<float,24,1> y_eq = outputMatrix * augmented_state_eq;
Eigen::Matrix<float,24,1> y_eq;
y_eq.setZero();
y_eq(3,0) = load_setpoint[0] + P1_world[0]; // equilibrium x-pose copter 1
y_eq(4,0) = load_setpoint[1] + P1_world[1]; // equilibrium y-pose copter 1
y_eq(5,0) = load_setpoint[2] + P1_world[2] + len_cable; // equilibrium z-pose copter 1
y_eq(11,0) = load_setpoint[0] + P2_world[0]; // equilibrium x-pose copter 2
y_eq(12,0) = load_setpoint[1] + P2_world[1]; // equilibrium y-pose copter 2
y_eq(13,0) = load_setpoint[2] + P2_world[2] + len_cable; // equilibrium z-pose copter 2
y_eq(19,0) = load_setpoint[0] + P3_world[0]; // equilibrium x-pose copter 3
y_eq(20,0) = load_setpoint[1] + P3_world[1]; // equilibrium y-pose copter 3
y_eq(21,0) = load_setpoint[2] + P3_world[2] + len_cable; // equilibrium z-pose copter 3
y_eq(3,0) = load_setpoint[0] + P1_world_eq[0]; // equilibrium x-pose copter 1
y_eq(4,0) = load_setpoint[1] + P1_world_eq[1]; // equilibrium y-pose copter 1
y_eq(5,0) = load_setpoint[2] + P1_world_eq[2] + len_cable; // equilibrium z-pose copter 1
y_eq(11,0) = load_setpoint[0] + P2_world_eq[0]; // equilibrium x-pose copter 2
y_eq(12,0) = load_setpoint[1] + P2_world_eq[1]; // equilibrium y-pose copter 2
y_eq(13,0) = load_setpoint[2] + P2_world_eq[2] + len_cable; // equilibrium z-pose copter 2
y_eq(19,0) = load_setpoint[0] + P3_world_eq[0]; // equilibrium x-pose copter 3
y_eq(20,0) = load_setpoint[1] + P3_world_eq[1]; // equilibrium y-pose copter 3
y_eq(21,0) = load_setpoint[2] + P3_world_eq[2] + len_cable; // equilibrium z-pose copter 3
// compute y w.r.t. equilibrium position, passed to controller
y_to_controller = y_meas - y_eq;
......
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