Commit d15141bc authored by mastefan's avatar mastefan
Browse files

Adding Flying Sequence

parent 0f991ae2
......@@ -167,6 +167,10 @@
// Namespacing the package
using namespace d_fall_pps;
// Flight sequences
#define SEQUENCE_NONE 0
#define SEQUENCE_LAND_ON_MOTHERSHIP 1
......@@ -189,10 +193,31 @@ float m_mass_CF_grams;
Setpoint dronexSetpoint;
int flying_state = DRONEX_STATE_GROUND;
int integratorFlag = INTEGRATOR_OFF;
int integratorFlag = DRONEX_INTEGRATOR_OFF;
float integrator_var[3] = {0,0,0};
// Variable for choosing flight sequence
int flightSequence;
// Flags of landing sequence
bool tookOffFlag = false;
bool approachedFlag = false;
bool landedFlag = false;
// Flag for saving starting coordinates
bool savedStartCoordinates;
int startCoordinateX;
int startCoordinateY;
int startCoordinateZ;
int eps_height = 0.05;
//describes the area of the crazyflie and other parameters
//CrazyflieContext context;
......
......@@ -202,14 +202,10 @@ void buttonPressed_take_off(){
void buttonPressed_land(){
//if(flying_state == DRONEX_STATE_HOVER){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Landing...");
flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
//}else{
// ROS_ERROR("Cannot change to DRONEX_STATE_LAND_ON_MOTHERSHIP");
//}
// flying_state = DRONEX_STATE_APPROACHING; iff already took off
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Start flight-sequence LA...");
// OLD:flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
// NEW:
flightSequence = 1;
}
void buttonPressed_abort(){
......@@ -409,20 +405,54 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
break;
case DRONEX_STATE_TAKING_OFF:
{
ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF");
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z +0.6;
if(!savedStartCoordinates)
{
startCoordinateY = request.ownCrazyflie.x; // Nöd sicher öbs das brucht. Idee: Afangscoordinate abspeichere zum in Hover state z ende.
startCoordinateX = request.ownCrazyflie.y;
startCoordinateZ = request.ownCrazyflie.z;
savedStartCoordinates = true;
//setpointCallback(dronexSetpoint);
}
}
dronexSetpoint.x = startCoordinateX;
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
}
//setpointCallback(dronexSetpoint);
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;
}
}
}
setpointCallback(dronexSetpoint);
......@@ -671,6 +701,7 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
//integrator for x,y,z
if(integratorFlag == DRONEX_INTEGRATOR_ON)
{
ROS_INFO_STREAM("DRONEX_INTEGRATOR_ON");
for(int i=0; i < 3; i++)
{
integrator_var[i] += stateErrorBody[i]*(1.0/control_frequency);
......
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