Commit 53888bd9 authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

Got UWB to work (yeeay)

parent 0e87f28d
......@@ -112,8 +112,6 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
ui->label_battery->setStyleSheet("QLabel { color : red; }");
m_battery_state = BATTERY_STATE_NORMAL;
// NEW STUFF
UWBServiceClient = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
......
......@@ -2,7 +2,7 @@ safeController: "SafeControllerService/RateController"
customController: "CustomControllerService/CustomController"
strictSafety: true
angleMargin: 0.6
battery_threshold_while_flying: 2.9 # in V
battery_threshold_while_flying: 2.7 # in V was 2.9
battery_threshold_while_motors_off: 3.34 # in V
battery_polling_period: 500 # in ms
positioning_polling_period: 10
positioning_polling_period: 10 # 10ms
#equlibrium offset
feedforwardMotor: [41000, 41000, 41000, 41000]
#equlibrium offset was 41
feedforwardMotor: [46000, 46000, 46000, 46000]
#quadratic motor regression equation (a0, a1, a2)
#motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
#feedback gain matrix
gainMatrixRoll: [0, -1.714330725, 0, 0, -1.337107465, 0, 5.115369735, 0, 0]
gainMatrixPitch: [1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0]
#gainMatrixRoll: [0, -1.714330725, 0, 0, -1.337107465, 0, 5.115369735, 0, 0]
#gainMatrixPitch: [1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0]
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]
gainMatrixYaw: [0, 0, 0, 0, 0, 0, 0, 0, 2.843099534]
gainMatrixThrust: [0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0]
......@@ -15,13 +19,14 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
#setpoint in meters (x, y, z, yaw)
defaultSetpoint: [0.0, 0.0, 0.4, 0.0]
defaultSetpoint: [0.0, 0.0, -0.2, 0.0]
#take off and landing parameters (in meters and seconds)
takeOffDistance : 0.4
landingDistance : 0.1
landingDistance : -0.5
durationTakeOff : 3
durationLanding : 3
# Liner Trayectory following parameters
distanceThreshold : 0.5
#distanceThreshold : 0.5
distanceThreshold : 0.2
......@@ -61,7 +61,6 @@ int main(int argc, char* argv[])
ros::Subscriber enableUWBSubscriber = namespaceNodeHandle.subscribe("student_GUI/uwbChanged", 1, uwbChangedCallback);
//ros::Subscriber enableUWBSubscriber = nodeHandle.subscribe("/my_GUI/enableUWB", 1, uwbChangedCallback);
ROS_WARN("Topic: %s", enableUWBSubscriber.getTopic().c_str());
loadUWBSettings();
// subscribe to the global vicon publisher and local uwb publisher
......@@ -137,6 +136,22 @@ void d_fall_pps::loadUWBSettings()
ROS_WARN("[LocalizationServer] UWB deactivated!");
}
void tiagosPrint(double x, double y, double z)
{
// Anchor2
//double anchorPos[3] = {1.89, 0.61, 1.6};
// Anchor5
double anchorPos[3] = {-0.76, -1.36, 0.6};
/*ROS_WARN("Dist to A5: %lf", sqrt(
(x - anchorPos[0])*(x - anchorPos[0])
+ (y - anchorPos[1])*(y - anchorPos[1])
+ (z - anchorPos[2])*(z - anchorPos[2])
)
);*/
}
void d_fall_pps::viconDataCallback(const ViconData &viconData)
{
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
......@@ -156,6 +171,8 @@ void d_fall_pps::viconDataCallback(const ViconData &viconData)
break;
}
}
tiagosPrint(CFData_vicon.x, CFData_vicon.y, CFData_vicon.z);
}
// Callback function to handle incoming UWB data
......@@ -164,10 +181,19 @@ void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
// override UWB Data
CrazyflieData CFData_uwb = data;
ROS_WARN("Roll Error: V-U = %f", CFData_vicon.roll - data.roll);
/* CFData_uwb.x = CFData_vicon.x;
CFData_uwb.y = CFData_vicon.y;*/
CFData_uwb.z = CFData_vicon.z;
CFData_uwb.y = CFData_vicon.y;
CFData_uwb.roll = CFData_vicon.roll;
*/
CFData_uwb.z = CFData_vicon.z;
CFData_uwb.pitch = CFData_vicon.pitch;
CFData_uwb.yaw = CFData_vicon.yaw;
......
......@@ -33,7 +33,11 @@
using namespace d_fall_pps;
// This is a test comment to check wtf git does
// Kalmanfilter Variables
#define KALMAN_M_VARIANCE 1 // was 1
#define KALMAN_P_VARIANCE 0.1
#define DISTANCE_ERROR_TRESHOLD 0.9
// the clientID defines namespace and identifier in the CentralManagerService
int clientID;
......@@ -50,12 +54,13 @@ double rollPitchYaw[3];
UWBAnchorArray anchors;
double anchorPositions[3*UWB_NUM_ANCHORS];
double lastDistances[UWB_NUM_ANCHORS] = {0};
KalmanState xState;
KalmanState yState;
KalmanState zState;
static KalmanState xState;
static KalmanState yState;
static KalmanState zState;
KalmanState kalmanDist[UWB_NUM_ANCHORS];
//KalmanState kalmanDist[UWB_NUM_ANCHORS];
int main(int argc, char* argv[])
{
......@@ -155,6 +160,8 @@ void d_fall_pps::cfDistancesCallback(const std_msgs::UInt32MultiArray &distances
UWBDataPublisher.publish(data);
}
//calculates position from anchor locations and distances -> forces algorithm?
void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, double(&xyz)[3])
{
......@@ -163,29 +170,44 @@ void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, doubl
for(unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
{
if(abs(lastDistances[i] - distances.data[i] / 100.0) > DISTANCE_ERROR_TRESHOLD)
ROS_WARN("Bad reading from Anchor %i", i+1);
dist[i] = distances.data[i] / 100.0; //in dm
lastDistances[i] = dist[i];
if(distances.data[i] == 0)
ROS_WARN("Bad reading from Anchor %i", i+1);
ROS_ERROR("Error with Anchor %i", i+1);
}
//filterDistances(dist);
/*for(int i = 0; i < UWB_NUM_ANCHORS; ++i)
{
ROS_WARN("Anchor %i: (%f, %f, %f)", i, anchorPositions[3*i], anchorPositions[3*i+1], anchorPositions[3*i+2]);
}*/
IndoorNewton(anchorPositions, dist, result, 50);
//ROS_INFO_STREAM(result[0]);
updateKalmanState(xState, result[0] / 10.0);
updateKalmanState(yState, result[1] / 10.0);
updateKalmanState(zState, result[2] / 10.0);
// Use this for Kalmanfiltered position
//xyz[0] = xState.x;
//xyz[1] = yState.x;
//xyz[2] = zState.x;
xyz[0] = xState.x;
xyz[1] = yState.x;
xyz[2] = zState.x;
//ROS_WARN("Kalman: (%f, %f, %f)", xState.x, yState.x, zState.x);
// Use this for unfiltered position
xyz[0] = result[0] / 10.0;
/*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);
}
......@@ -213,13 +235,13 @@ void d_fall_pps::publishLog(const std_msgs::UInt32MultiArray& distances, const d
void d_fall_pps::filterDistances(double* distances)
{
for (unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
/*for (unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
{
if(distances[i] != 0)
updateKalmanState(kalmanDist[i], distances[i]);
distances[i] = kalmanDist[i].x;
}
}*/
}
void d_fall_pps::initKalmanStates()
......@@ -227,31 +249,31 @@ void d_fall_pps::initKalmanStates()
// Init x state
// TODO Update
xState.x = 0;
xState.q = 0.005;
xState.r = 6;
xState.q = KALMAN_P_VARIANCE;
xState.r = KALMAN_M_VARIANCE;
xState.p = 0;
// Init y state
// TODO Update
yState.x = 0;
yState.q = 0.005;
yState.r = 6;
yState.q = KALMAN_P_VARIANCE;
yState.r = KALMAN_M_VARIANCE;
yState.p = 0;
// Init z state
// TODO Update
zState.x = 0;
zState.q = 0.005;
zState.r = 6;
zState.q = KALMAN_P_VARIANCE;
zState.r = KALMAN_M_VARIANCE;
zState.p = 0;
for(unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
/*for(unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
{
kalmanDist[i].x = 0;
kalmanDist[i].q = 0.005;
kalmanDist[i].r = 1;
kalmanDist[i].p = 0;
}
}*/
}
void d_fall_pps::updateKalmanState(KalmanState& state, double measurment)
......
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