To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 2de29a99 authored by remartin's avatar remartin
Browse files

Some old changes which were not commited.

parent a650984f
......@@ -31,18 +31,25 @@ landingDistance : -0.5
################### UWB Settings
#equlibrium offset
feedforwardMotor: [43000, 43000, 43000, 43000]
feedforwardMotor: [43500, 43500, 43500, 43500]
#feedforwardMotor: [36000, 36000, 36000, 36000] # No Markers
#quadratic motor regression equation (a0, a1, a2)
#motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
#feedback gain matrix changed
gainMatrixRoll: [0, -1.3, 0, 0, -0.9, 0, 4.715369735, 0, 0]
gainMatrixPitch: [1.3, 0, 0, 0.9, 0, 0, 0, 4.715369735, 0]
gainMatrixRoll: [0, -1.1, 0, 0, -0.7, 0, 4.715369735, 0, 0]
gainMatrixPitch: [1.1, 0, 0, 0.7, 0, 0, 0, 4.715369735, 0]
#gainMatrixYaw: [0, 0, 0, 0, 0, 0, 0, 0, 2.843099534]
gainMatrixThrust: [0, 0, 0.15, 0, 0, 0.06, 0, 0, 0]
#feedback gain matrix changed -> No Markers
#gainMatrixRoll: [0, -1.0, 0, 0, -0.6, 0, 4.715369735, 0, 0] # No Markers
#gainMatrixPitch: [1.0, 0, 0, 0.6, 0, 0, 0, 4.715369735, 0] # No Markers
#gainMatrixYaw: [0, 0, 0, 0, 0, 0, 0, 0, 2.843099534] # No Markers
#gainMatrixThrust: [0, 0, 0.13, 0, 0, 0.06, 0, 0, 0] # No Markers
#kalman filter
#filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
#estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
......@@ -57,4 +64,4 @@ durationTakeOff : 1
durationLanding : 2
# Liner Trayectory following parameters
distanceThreshold : 0.3
distanceThreshold : 0.2
\ No newline at end of file
......@@ -41,7 +41,8 @@ using namespace d_fall_pps;
#define DISTANCE_ERROR_TRESHOLD 0.9
#define ANCHOR_INDEX_TOP 4
#define ANCHOR_HEIGHT_TOP 17.0 //anchors.data[4].z //in dm
//#define ANCHOR_HEIGHT_TOP -12.8 //in dm FOR FLOOR
//#define ANCHOR_HEIGHT_TOP 17.0 //in dm FOR CEILING
// the clientID defines namespace and identifier in the CentralManagerService
int clientID;
......@@ -270,7 +271,10 @@ void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, doubl
double delat_x_2 = (anchorPositions[3*ANCHOR_INDEX_TOP] - result[0]) * (anchorPositions[3*ANCHOR_INDEX_TOP] - result[0]);
double delta_y_2 = (anchorPositions[3*ANCHOR_INDEX_TOP+1] - result[1]) * (anchorPositions[3*ANCHOR_INDEX_TOP+1] - result[1]);
double cf_height = std::sqrt(std::abs(dist[ANCHOR_INDEX_TOP]*dist[ANCHOR_INDEX_TOP] - delat_x_2 - delta_y_2));
result[2] = ANCHOR_HEIGHT_TOP - cf_height;
//result[2] = ANCHOR_HEIGHT_TOP + cf_height; //FOR FLOOR
result[2] = anchorPositions[3*ANCHOR_INDEX_TOP+2] - cf_height; //FOR CEILING
//ANCHOR_HEIGHT_TOP
updateKalmanState(xState, result[0] / 10.0);
updateKalmanState(yState, result[1] / 10.0);
......@@ -279,14 +283,12 @@ void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, doubl
// Use this for Kalmanfiltered position
xyz[0] = xState.x;
xyz[1] = yState.x;
xyz[2] = zState.x;
//ROS_WARN("Kalman: (%f, %f, %f)", xState.x, yState.x, zState.x);
//xyz[2] = zState.x;
// Use this for unfiltered position
// xyz[0] = result[0] / 10.0;
// xyz[1] = result[1] / 10.0;
//xyz[2] = result[2] / 10.0;
xyz[2] = result[2] / 10.0;
//publishLog(distances, dist);
}
......
Markdown is supported
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