Commit 9335b065 authored by mastefan's avatar mastefan
Browse files

Small changes

parent dda9d29e
......@@ -169,8 +169,9 @@
using namespace d_fall_pps;
// Flight sequences
#define SEQUENCE_NONE 0
#define SEQUENCE_LAND_ON_MOTHERSHIP 1
#define SEQUENCE_NONE 0
#define SEQUENCE_LAND_ON_MOTHERSHIP 1
#define SEQUENCE_HOVER_OVER_MOTHERSHIP 2
......@@ -215,7 +216,7 @@ float startCoordinateY;
float startCoordinateZ;
// tolerances (x,y,z)
float tol_eps[3] = {0.5, 0.5, 0.5};
float tol_eps[3] = {0.03, 0.03, 0.03};
float eps_height = 0.05;
......
......@@ -48,7 +48,7 @@ shouldDisplayDebugInfo : false
# - Swaps between pitch set-points to test angle set-point response time
# i.e., this controller test the assumption that "the inner loop is infinitely fast"
#
controller_mode : 3
controller_mode : 6
# A flag for which estimator to use, defined as:
......@@ -124,4 +124,4 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
#PMKF_Ahat_row1_for_angles : [ 0.6723, 0.0034]
#PMKF_Ahat_row2_for_angles : [-12.9648, 0.9352]
#PMKF_Kinf_for_angles : [ 0.3277,12.9648]
\ No newline at end of file
#PMKF_Kinf_for_angles : [ 0.3277,12.9648]
......@@ -3,8 +3,8 @@ mass_CF : 28
# Max setpoint change per second
max_setpoint_change_per_second_horizontal : 1.00 # [meters]
max_setpoint_change_per_second_vertical : 0.40 # [meters]
max_setpoint_change_per_second_horizontal : 2.00 # [meters]
max_setpoint_change_per_second_vertical : 0.8 # [meters]
max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
# Frequency of the controller, in hertz
......
......@@ -364,7 +364,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//ROS_INFO("DRONEX_STATE_APPROACH");
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.6;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.4;//0.6;
if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_eps[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_eps[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_eps[2] ){
......@@ -424,7 +424,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;
}
break;
......@@ -441,7 +441,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 = 0;
/*if(request.ownCrazyflie.z < request.otherCrazyflies[0].z+0.25){ // >0.1
......@@ -497,7 +497,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.z = startCoordinateZ + 0.6;
if(abs(request.ownCrazyflie.z - startCoordinateZ - 0.6) < eps_height){
if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_eps[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_eps[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_eps[2]) {
ROS_INFO("took off");
tookOffFlag = true;
flying_state = DRONEX_STATE_HOVER;
......@@ -578,11 +579,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Turn motors off if wanted or do LQR-control
if(flying_state == DRONEX_STATE_LAND_ON_GROUND && (request.ownCrazyflie.z < 0.1 )){
if(flying_state == DRONEX_STATE_LAND_ON_GROUND && (request.ownCrazyflie.z < 0.05 )){
ROS_INFO("landed -> DRONEX_STATE_ON_GROUND");
flying_state = DRONEX_STATE_GROUND;
}
else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP && (request.ownCrazyflie.z < 0.1 + request.otherCrazyflies[0].z)){
else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP && (abs(request.ownCrazyflie.x - request.otherCrazyflies[0].x) < tol_eps[0]) &&
(abs(request.ownCrazyflie.y - request.otherCrazyflies[0].y) < tol_eps[1]) &&
(abs(request.ownCrazyflie.z - 0.05 - request.otherCrazyflies[0].z) < tol_eps[2]) ){
ROS_INFO("landed -> DRONEX_STATE_ON_MOTHERSHIP");
flying_state = DRONEX_STATE_ON_MOTHERSHIP;
}
......@@ -946,7 +949,9 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
// The "DebugMsg" type has 10 properties from "value_1" to "value_10", all of
// type "float64" that you can fill in with data you would like to plot in
// real-time.
// debugMsg.value_1 = thrustAdjustment;
debugMsg.value_1 = drone_X_vel[0];
debugMsg.value_2 = drone_X_vel[1];
debugMsg.value_3 = drone_X_vel[2];
// ......................
// debugMsg.value_10 = your_variable_name;
......
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