Commit f1cae724 authored by phfriedl's avatar phfriedl
Browse files

added student controller framework called CustomControllerService

parent f0fb2d88
......@@ -5,4 +5,4 @@ float64 z
float64 roll
float64 pitch
float64 yaw
float64 acquiringTime
float64 acquiringTime #delta t
......@@ -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.3, 1]
defaultSetpoint: [0.0, 0.0, 0.3, 0.0]
......@@ -179,9 +179,7 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
}
*/
void convertIntoBodyFrame(Controller::Request &request, float est[9], float (&state)[9], int yaw_measured) {
//float sinYaw = sin(request.crazyflieLocation.yaw);
//float cosYaw = cos(request.crazyflieLocation.yaw);
void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
float sinYaw = sin(yaw_measured);
float cosYaw = cos(yaw_measured);
......@@ -219,8 +217,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
estimateState(request, est);
float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
convertIntoBodyFrame(request, est, state, yaw_measured);
//convertIntoBodyFrame(request, est, state, yaw);
convertIntoBodyFrame(est, state, yaw_measured);
//convertIntoBodyFrame(est, state, yaw);
//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