Commit fed7aba7 authored by maruggv's avatar maruggv
Browse files

not much ;)

parent 12c2fc73
......@@ -622,35 +622,15 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// > this function performs all estimation and control
// PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
// > After this function is complete the class variable
// "current_stateInertialEstimate" is updated and ready
// to be used for subsequent controller copmutations
performEstimatorUpdate_forStateInterial(request);
if(controller_mode == 0){ // lqr controller
if(controller_mode == 0){
// do not change
// do not change, use setpoint defined in states
}else if(controller_mode == 1){// controller that uses the mothership velocity
}else if(controller_mode == 1){ // nested lqr controller
//dronexSetpoint.x = dronexSetpoint.x;
//dronexSetpoint.y = dronexSetpoint.y;
//dronexSetpoint.z = dronexSetpoint.z;
dronexSetpoint.yaw = request.otherCrazyflies[0].yaw; // same yaw as mothership
// Load Mothership velocity
// x dot, y dot, z dot
dronexVelocity.x = mothership_vel[0];
dronexVelocity.y = mothership_vel[1];
dronexVelocity.z = mothership_vel[2];
}else if(controller_mode == 2){// Trajectory Follower
if(flying_state == DRONEX_STATE_FOLLOWING_TRAJECTORY){ // Trajectory Follower
// to implement the trajectory tracking
if(flying_state == DRONEX_STATE_FOLLOWING_TRAJECTORY){
calculateTrajectory(request);
dronexSetpoint.x = trajectory_setpoint[0];
......@@ -673,7 +653,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
m_velocity_for_controller_2[2] = 0;
*/
}else{
}else{ // standard: if not following trajectory
//dronexSetpoint.x = dronexSetpoint.x;
//dronexSetpoint.y = dronexSetpoint.y;
//dronexSetpoint.z = dronexSetpoint.z;
......@@ -687,17 +668,20 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
setpointCallback(dronexSetpoint, dronexVelocity);
// CALL THE FUNCTION FOR PER CYLCE OPERATIONS
// PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
// > After this function is complete the class variable
// "current_stateInertialEstimate" is updated and ready
// to be used for subsequent controller copmutations
performEstimatorUpdate_forStateInterial(request);
// CALL THE FUNCTION FOR PER CYLCE OPERATIONS: limits setpoint changes and velocity for controller
perControlCycleOperations();
float stateErrorBody[12];
// > Call the function to perform the conversion
float stateErrorBody[12];
convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, m_setpoint_for_controller, stateErrorBody);
// CARRY OUT THE CONTROLLER COMPUTATIONS
// Call the function that performs the control computations for this mode
......@@ -718,12 +702,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
motorsOFF(response);
}
else{
calculateControlOutputDroneX(request, response, stateErrorBody);
// for debugging:
/*ROS_INFO_STREAM("deltaX to Mothership: " << request.ownCrazyflie.x - request.otherCrazyflies[0].x);
ROS_INFO_STREAM("deltaY to Mothership: " << request.ownCrazyflie.y - request.otherCrazyflies[0].y);
ROS_INFO_STREAM("deltaZ to Mothership: " << request.ownCrazyflie.z - request.otherCrazyflies[0].z);*/
calculateControlOutputDroneX(request, response, stateErrorBody); // chooses controller mode
}
......@@ -772,38 +751,25 @@ void crazyfliecontextRefresh(d_fall_pps::CrazyflieContext context){
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response, float stateErrorBody[12]){
if(controller_mode == 0){
// CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO
// THE BODY FRAME ERROR REQUIRED BY THE CONTROLLER
// integrator
integrator_XYZ(stateErrorBody);
// Compute control output via LQR controller:
calculateControlOutput_viaLQRforRates(stateErrorBody,request,response);
}else if(controller_mode == 1){
//integrator
integrator_XYZ(stateErrorBody);
// Compute control output via Nested LQR controller:
calculateControlOutput_viaLQRforRates_Nested(stateErrorBody, request, response);
}else if(controller_mode == 2){// Trajectory Follower
// to implement the trajectory tracking
// integrator
integrator_XYZ(stateErrorBody);
// Compute control output via Nested LQR controller:
calculateControlOutput_viaLQRforRates_Nested(stateErrorBody, request, response);
}else{ // Don't know what to do ^^
ROS_ERROR("[DRONEXCONTROLLERSERVICE] Please change the controller mode");
}
}
......
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