Commit bea40b0d authored by maruggv's avatar maruggv
Browse files

added yaml import for sequence, make use of constant defs

parent d93b0161
......@@ -107,10 +107,10 @@
#define DRONEX_INTEGRATOR_OFF 16
#define DRONEX_INTEGRATOR_RESET 17
#define DRONEX_STATE_GROUND 0
#define DRONEX_STATE_TAKING_OFF 1
#define DRONEX_STATE_LAND_ON_GROUND 2
#define DRONEX_STATE_APPROACH 3
#define DRONEX_STATE_GROUND 0
#define DRONEX_STATE_TAKING_OFF 1
#define DRONEX_STATE_LAND_ON_GROUND 2
#define DRONEX_STATE_APPROACH 3
#define DRONEX_STATE_HOVER 4
#define DRONEX_STATE_LAND_ON_MOTHERSHIP 5
#define DRONEX_STATE_ON_MOTHERSHIP 6
......@@ -201,7 +201,7 @@ float integrator_var[3] = {0,0,0};
// Variable for choosing flight sequence
int flightSequence = 0;
int flightSequence = SEQUENCE_NONE;
// Flags of landing sequence
bool tookOffFlag = false;
......@@ -217,6 +217,7 @@ float startCoordinateZ;
// tolerances (x,y,z)
float tol_eps[3] = {0.5, 0.5, 0.5};
float eps_height = 0.05;
float startHeight = 0.6;
......
# Mass of the crazyflie
mass_CF : 28
mass_CF : 32
# Max setpoint change per second
......@@ -85,3 +85,5 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
# for our integrator (so far just random values)
gainIntegrator : [1, 1, 1]
# flightSequence : 0
......@@ -416,7 +416,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//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;
dronexSetpoint.z = request.otherCrazyflies[0].z + eps_height;
}
break;
......@@ -433,7 +433,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// to land on ground (17.10. vm)
dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = 0.05;
dronexSetpoint.z = eps_height;
/*if(request.ownCrazyflie.z < request.otherCrazyflies[0].z+0.25){ // >0.1
......@@ -486,10 +486,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.x = startCoordinateX;
dronexSetpoint.y = startCoordinateY;
dronexSetpoint.z = startCoordinateZ + 0.6;
dronexSetpoint.z = startCoordinateZ + startHeight;
if(abs(request.ownCrazyflie.z - startCoordinateZ - 0.6) < eps_height){
if(abs(request.ownCrazyflie.z - startCoordinateZ - startHeight) < eps_height){
ROS_INFO("took off");
tookOffFlag = true;
flying_state = DRONEX_STATE_HOVER;
......@@ -515,7 +515,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if (flightSequence == 1){
if (flightSequence == SEQUENCE_LAND_ON_MOTHERSHIP){
flying_state = DRONEX_STATE_TAKING_OFF;
//ROS_INFO_STREAM("Flight sequence: Landing on mothership");
if(tookOffFlag){
......@@ -1353,6 +1353,9 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// for our integrator
getParameterFloatVector(nodeHandle_for_dronexController, "gainIntegrator", gainIntegrator, 3);
// load flightSequence
// flightSequence = getParameterInt(nodeHandle_for_dronexController, "flightSequence");
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[DRONEX CONTROLLER] DEBUGGING: the fetched DroneXController/mass_CF = " << m_mass_CF_grams);
......
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