Commit ce1be7ea authored by maruggv's avatar maruggv
Browse files

Anpassungen für LAND_ON_GROUND und LAND_ON_MOTHERSHIP

parent d15141bc
......@@ -200,7 +200,7 @@ float integrator_var[3] = {0,0,0};
// Variable for choosing flight sequence
int flightSequence;
int flightSequence = 0;
// Flags of landing sequence
bool tookOffFlag = false;
......@@ -208,7 +208,7 @@ bool approachedFlag = false;
bool landedFlag = false;
// Flag for saving starting coordinates
bool savedStartCoordinates;
bool savedStartCoordinates = false;
int startCoordinateX;
int startCoordinateY;
int startCoordinateZ;
......
......@@ -349,33 +349,39 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
switch(flying_state){
case DRONEX_STATE_APPROACH:
{
ROS_INFO("DRONEX_STATE_APPROACH");
{
ROS_INFO("DRONEX_STATE_APPROACH");
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.2;
//setpointCallback(dronexSetpoint);
}
break;
break;
case DRONEX_STATE_GROUND:
//{
ROS_INFO("DRONEX_STATE_GROUND");
{
ROS_INFO("DRONEX_STATE_GROUND");
/* std_msgs::Int32 flying_state_msg;
flying_state_msg.data = STATE_MOTORS_OFF;
flyingStatePublisher.publish(flying_state_msg);
return true;
}*/
break;
return true;*/
}
break;
case DRONEX_STATE_LAND_ON_GROUND:
{
{
//ROS_INFO_STREAM("DRONEX_STATE_LAND_ON_GROUND: Crazyflie z coordinate " << request.ownCrazyflie.z);
//ROS_INFO_STREAM("DRONEX_STATE_LAND_ON_GROUND: Mothership z coordinate " << request.otherCrazyflies[0].z);
dronexSetpoint.x = request.otherCrazyflies[0].x; //set setpoint to droneX x y and z coordinates
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z+0.05;
// to land on mothership: -> commented out (17.10. vm)
//dronexSetpoint.x = request.otherCrazyflies[0].x; //set setpoint to droneX x y and z coordinates
//dronexSetpoint.y = request.otherCrazyflies[0].y;
//dronexSetpoint.z = request.otherCrazyflies[0].z+0.05;
// to land on ground (17.10. vm)
dronexSetpoint.x = request.ownCrazyflie[0].x;
dronexSetpoint.y = request.ownCrazyflie[0].y;
dronexSetpoint.z = 0.05;
/*if(request.ownCrazyflie.z < request.otherCrazyflies[0].z+0.25){ // >0.1
......@@ -404,11 +410,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
break;
}
break;
case DRONEX_STATE_TAKING_OFF:
{
{
ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF");
if(!savedStartCoordinates)
......@@ -417,42 +423,50 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
startCoordinateX = request.ownCrazyflie.y;
startCoordinateZ = request.ownCrazyflie.z;
savedStartCoordinates = true;
//setpointCallback(dronexSetpoint);
}
}
dronexSetpoint.x = startCoordinateX;
dronexSetpoint.y = startCoordinateY;
dronexSetpoint.z = startCoordinateZ + 0.6;
dronexSetpoint.y = startCoordinateY;
dronexSetpoint.z = startCoordinateZ + 0.6;
if((request.ownCrazyflie.z > startCoordinateZ + 0.6 - eps_height) || (request.ownCrazyflie.z < startCoordinateZ + 0.6 + eps_height)){
tookOffFlag == true;
// -> flyingState: Hover
}
}
break;
case DRONEX_STATE_LAND_ON_MOTHERSHIP:
{
//to land on mothership (17.10. vm)
dronexSetpoint.x = request.otherCrazyflies[0].x; //set setpoint to droneX x y and z coordinates
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z+0.05;
}
break;
} // END switch case
break;
} // END switch case
if (flightSequence == 1){
flying_state = DRONEX_TAKE_OFF;
//ROS_INFO_STREAM("Flight sequence: Landing on mothership");
if(tookOffFlag){
flying_state = DRONEX_STATE_APPROACH;
if(approachedFlag){
flying_state = DRONEX_LAND;
}
flying_state = DRONEX_TAKE_OFF;
//ROS_INFO_STREAM("Flight sequence: Landing on mothership");
if(tookOffFlag){
flying_state = DRONEX_STATE_APPROACH;
if(approachedFlag){
flying_state = DRONEX_LAND; // @ Stefano: Was macht DRONEX_LAND? müessti das nid DRONEX_LAND_ON_MOTHERSHIP si?
// Valentin
}
}
}
setpointCallback(dronexSetpoint);
......@@ -464,7 +478,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//setpointCallback(dronexSetpoint);
// CALL THE FUNCTION FOR PER CYLCE OPERATIONS
perControlCycleOperations();
perControlCycleOperations();
// THIS IS THE START OF THE "OUTER" CONTROL LOOP
// > i.e., this is the control loop run on this laptop
......@@ -491,14 +505,19 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// CARRY OUT THE CONTROLLER COMPUTATIONS
// Call the function that performs the control computations for this mode
if((flying_state == DRONEX_STATE_LAND_ON_GROUND && (request.ownCrazyflie.z < 0.1 + request.otherCrazyflies[0].z)) || flying_state == DRONEX_STATE_GROUND){
// Turn motors off if wanted or do LQR-control
if(flying_state == DRONEX_STATE_LAND_ON_GROUND && (request.ownCrazyflie.z < 0.1 )){
motorsOFF(response);
}else{
}
else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP && (request.ownCrazyflie.z < 0.1 + request.otherCrazyflies[0].z)){
motorsOFF(response);
}
else if(flying_state == DRONEX_STATE_GROUND){
motorsOFF(response);
}
else{
calculateControlOutput_viaLQRforRates(current_bodyFrameError,request,response);
}
......
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