Commit 9c237714 authored by ucart's avatar ucart

Can calculate distance to 1-4 quads at once

parent ef5fa643
......@@ -40,11 +40,13 @@ class Bridge {
* Calculates the distance between the Bridge and each of the active Crazyflie
*
* @param pos Pointer to a lateral position struct
* @param dis Distance between Bridge and Quadcopter1
* @param dis Distance between Bridge and Quadcopters, up to four
* @return True if a location is valid, false if no location is able to found
*/
bool getDistanceToQuad1(lateralPosition *pos, targetDistance *disToQuad1);
bool getTargetDistance(lateralPosition *pos, targetDistance *disToQuad1);
bool getTargetDistance(lateralPosition *pos, targetDistance *disToQuad1, targetDistance *disToQuad2);
bool getTargetDistance(lateralPosition *pos, targetDistance *disToQuad1, targetDistance *disToQuad2, targetDistance *disToQuad3);
bool getTargetDistance(lateralPosition *pos, targetDistance *disToQuad1, targetDistance *disToQuad2, targetDistance *disToQuad3, targetDistance *disToQuad4);
/**
* Callbacks for the VRPN trackers on either end of the bridge
*/
......
......@@ -14,7 +14,7 @@
#define NUM_RADIOS 1
// The number of quadcopters to initialize
#define NUM_QUADS 1
#define NUM_QUADS 2
#define BASE_THRUST 42000
#define Kp 5000
......
......@@ -132,6 +132,7 @@ typedef struct targetDistance {
float x;
float y;
float z;
float dis;
} targetDistance;
typedef struct quaternionPosition {
......
......@@ -3,6 +3,7 @@
#include "structures.h"
#include "quadcopterData.h"
#include "crazyflieGroundStation.h"
#include <cmath>
Bridge::Bridge() {
// The endpoints are not valid yet
......@@ -51,38 +52,107 @@ bool Bridge::getInterpolatedPosition(lateralPosition *pos, int loc) {
return( retVal );
}
bool Bridge::getTargetDistance(lateralPosition *pos, targetDistance *disToQuad1
/*, targetDistance *disToQuad2, targetDistance *disToQuad3, targetDistance *disToQuad4*/) {
/**
* Calculates the distance between the Bridge and each of the active Crazyflie
*
* @param pos Pointer to a lateral position struct
* @param dis Distance between Bridge and Quadcopters, up to four
* @return True if a location is valid, false if no location is able to found
*/
bool Bridge::getTargetDistance(lateralPosition *pos, targetDistance *disToQuad1) {
bool retVal = false;
if(this->endpoint1Valid){
if(NUM_QUADS){
coordinateSystem coor = crazyflie_info[0].cflieCopter->getCoordinateSystem();
this->disToQuad1.x = this->endpoint1.x - crazyflie_info[0].cflieCopter->getPositionX( coor );
this->disToQuad1.y = this->endpoint1.y - crazyflie_info[0].cflieCopter->getPositionY( coor );
this->disToQuad1.z = this->endpoint1.z - crazyflie_info[0].cflieCopter->getPositionZ( coor );
}
/* if(NUM_QUADS > 1) {
coordinateSystem coor = crazyflie_info[1].cflieCopter->getCoordinateSystem();
disToQuad2->x = this->endpoint1.x - crazyflie_info[2].cflieCopter->getPositionX( coor );
disToQuad2->y = this->endpoint1.y - crazyflie_info[2].cflieCopter->getPositionY( coor );
disToQuad2->z = this->endpoint1.z - crazyflie_info[2].cflieCopter->getPositionZ( coor );
} if(NUM_QUADS > 2) {
coordinateSystem coor = crazyflie_info[2].cflieCopter->getCoordinateSystem();
disToQuad3->x = this->endpoint1.x - crazyflie_info[3].cflieCopter->getPositionX( coor );
disToQuad3->y = this->endpoint1.y - crazyflie_info[3].cflieCopter->getPositionY( coor );
disToQuad3->z = this->endpoint1.z - crazyflie_info[3].cflieCopter->getPositionZ( coor );
} if(NUM_QUADS > 3) {
coordinateSystem coor = crazyflie_info[3].cflieCopter->getCoordinateSystem();
disToQuad4->x = this->endpoint1.x - crazyflie_info[4].cflieCopter->getPositionX( coor );
disToQuad4->y = this->endpoint1.y - crazyflie_info[4].cflieCopter->getPositionY( coor );
disToQuad4->z = this->endpoint1.z - crazyflie_info[4].cflieCopter->getPositionZ( coor );
}
*/
this->disToQuad1.dis = ((this->disToQuad1.x)*(this->disToQuad1.x))+((this->disToQuad1.y)*(this->disToQuad1.y))+((this->disToQuad1.z)*(this->disToQuad1.z));
this->disToQuad1.dis = sqrt(this->disToQuad1.dis);
retVal = true;
}
return retVal;
}
bool Bridge::getTargetDistance(lateralPosition *pos, targetDistance *disToQuad1, targetDistance *disToQuad2) {
bool retVal = false;
if(this->endpoint1Valid){
coordinateSystem coor0 = crazyflie_info[0].cflieCopter->getCoordinateSystem();
this->disToQuad1.x = this->endpoint1.x - crazyflie_info[0].cflieCopter->getPositionX( coor0 );
this->disToQuad1.y = this->endpoint1.y - crazyflie_info[0].cflieCopter->getPositionY( coor0 );
this->disToQuad1.z = this->endpoint1.z - crazyflie_info[0].cflieCopter->getPositionZ( coor0 );
this->disToQuad1.dis = ((this->disToQuad1.x)*(this->disToQuad1.x))+((this->disToQuad1.y)*(this->disToQuad1.y))+((this->disToQuad1.z)*(this->disToQuad1.z));
this->disToQuad1.dis = sqrt(this->disToQuad1.dis);
coordinateSystem coor1 = crazyflie_info[1].cflieCopter->getCoordinateSystem();
this->disToQuad2.x = this->endpoint1.x - crazyflie_info[1].cflieCopter->getPositionX( coor1 );
this->disToQuad2.y = this->endpoint1.y - crazyflie_info[1].cflieCopter->getPositionY( coor1 );
this->disToQuad2.z = this->endpoint1.z - crazyflie_info[1].cflieCopter->getPositionZ( coor1 );
this->disToQuad2.dis = ((this->disToQuad2.x)*(this->disToQuad2.x))+((this->disToQuad2.y)*(this->disToQuad2.y))+((this->disToQuad2.z)*(this->disToQuad2.z));
this->disToQuad2.dis = sqrt(this->disToQuad2.dis);
retVal = true;
}
return retVal;
}
bool Bridge::getTargetDistance(lateralPosition *pos, targetDistance *disToQuad1, targetDistance *disToQuad2, targetDistance *disToQuad3) {
bool retVal = false;
if(this->endpoint1Valid){
coordinateSystem coor0 = crazyflie_info[0].cflieCopter->getCoordinateSystem();
this->disToQuad1.x = this->endpoint1.x - crazyflie_info[0].cflieCopter->getPositionX( coor0 );
this->disToQuad1.y = this->endpoint1.y - crazyflie_info[0].cflieCopter->getPositionY( coor0 );
this->disToQuad1.z = this->endpoint1.z - crazyflie_info[0].cflieCopter->getPositionZ( coor0 );
this->disToQuad1.dis = ((this->disToQuad1.x)*(this->disToQuad1.x))+((this->disToQuad1.y)*(this->disToQuad1.y))+((this->disToQuad1.z)*(this->disToQuad1.z));
this->disToQuad1.dis = sqrt(this->disToQuad1.dis);
coordinateSystem coor1 = crazyflie_info[1].cflieCopter->getCoordinateSystem();
this->disToQuad2.x = this->endpoint1.x - crazyflie_info[1].cflieCopter->getPositionX( coor1 );
this->disToQuad2.y = this->endpoint1.y - crazyflie_info[1].cflieCopter->getPositionY( coor1 );
this->disToQuad2.z = this->endpoint1.z - crazyflie_info[1].cflieCopter->getPositionZ( coor1 );
this->disToQuad2.dis = ((this->disToQuad2.x)*(this->disToQuad2.x))+((this->disToQuad2.y)*(this->disToQuad2.y))+((this->disToQuad2.z)*(this->disToQuad2.z));
this->disToQuad2.dis = sqrt(this->disToQuad2.dis);
coordinateSystem coor2 = crazyflie_info[2].cflieCopter->getCoordinateSystem();
this->disToQuad3.x = this->endpoint1.x - crazyflie_info[2].cflieCopter->getPositionX( coor2 );
this->disToQuad3.y = this->endpoint1.y - crazyflie_info[2].cflieCopter->getPositionY( coor2 );
this->disToQuad3.z = this->endpoint1.z - crazyflie_info[2].cflieCopter->getPositionZ( coor2 );
this->disToQuad3.dis = ((this->disToQuad3.x)*(this->disToQuad3.x))+((this->disToQuad3.y)*(this->disToQuad3.y))+((this->disToQuad3.z)*(this->disToQuad3.z));
this->disToQuad3.dis = sqrt(this->disToQuad3.dis);
retVal = true;
}
return retVal;
}
bool Bridge::getTargetDistance(lateralPosition *pos, targetDistance *disToQuad1, targetDistance *disToQuad2, targetDistance *disToQuad3, targetDistance *disToQuad4) {
bool retVal = false;
if(this->endpoint1Valid){
coordinateSystem coor0 = crazyflie_info[0].cflieCopter->getCoordinateSystem();
this->disToQuad1.x = this->endpoint1.x - crazyflie_info[0].cflieCopter->getPositionX( coor0 );
this->disToQuad1.y = this->endpoint1.y - crazyflie_info[0].cflieCopter->getPositionY( coor0 );
this->disToQuad1.z = this->endpoint1.z - crazyflie_info[0].cflieCopter->getPositionZ( coor0 );
this->disToQuad1.dis = ((this->disToQuad1.x)*(this->disToQuad1.x))+((this->disToQuad1.y)*(this->disToQuad1.y))+((this->disToQuad1.z)*(this->disToQuad1.z));
this->disToQuad1.dis = sqrt(this->disToQuad1.dis);
coordinateSystem coor1 = crazyflie_info[1].cflieCopter->getCoordinateSystem();
this->disToQuad2.x = this->endpoint1.x - crazyflie_info[1].cflieCopter->getPositionX( coor1 );
this->disToQuad2.y = this->endpoint1.y - crazyflie_info[1].cflieCopter->getPositionY( coor1 );
this->disToQuad2.z = this->endpoint1.z - crazyflie_info[1].cflieCopter->getPositionZ( coor1 );
this->disToQuad2.dis = ((this->disToQuad2.x)*(this->disToQuad2.x))+((this->disToQuad2.y)*(this->disToQuad2.y))+((this->disToQuad2.z)*(this->disToQuad2.z));
this->disToQuad2.dis = sqrt(this->disToQuad2.dis);
coordinateSystem coor2 = crazyflie_info[2].cflieCopter->getCoordinateSystem();
this->disToQuad3.x = this->endpoint1.x - crazyflie_info[2].cflieCopter->getPositionX( coor2 );
this->disToQuad3.y = this->endpoint1.y - crazyflie_info[2].cflieCopter->getPositionY( coor2 );
this->disToQuad3.z = this->endpoint1.z - crazyflie_info[2].cflieCopter->getPositionZ( coor2 );
this->disToQuad3.dis = ((this->disToQuad3.x)*(this->disToQuad3.x))+((this->disToQuad3.y)*(this->disToQuad3.y))+((this->disToQuad3.z)*(this->disToQuad3.z));
this->disToQuad3.dis = sqrt(this->disToQuad3.dis);
coordinateSystem coor3 = crazyflie_info[3].cflieCopter->getCoordinateSystem();
this->disToQuad4.x = this->endpoint1.x - crazyflie_info[3].cflieCopter->getPositionX( coor3 );
this->disToQuad4.y = this->endpoint1.y - crazyflie_info[3].cflieCopter->getPositionY( coor3 );
this->disToQuad4.z = this->endpoint1.z - crazyflie_info[3].cflieCopter->getPositionZ( coor3 );
this->disToQuad4.dis = ((this->disToQuad4.x)*(this->disToQuad4.x))+((this->disToQuad4.y)*(this->disToQuad4.y))+((this->disToQuad4.z)*(this->disToQuad4.z));
this->disToQuad4.dis = sqrt(this->disToQuad4.dis);
retVal = true;
}
return retVal;
}
/**
* Callbacks for the VRPN trackers on either end of the bridge
*/
......
......@@ -19,32 +19,15 @@ void* displayData(void* id) {
printf("Spawning User Output Thread\n");
lateralPosition bridgePos;
targetDistance pos1, pos2, pos3, pos4;
while (1) {
// Clear the display
cout << CLEAR;
// Print the bridge
#ifdef USE_BRIDGE
if(bridge->endpoint1Valid){
cout << "Target 1 X: " << bridge->endpoint1.x << "\tY: " << bridge->endpoint1.y << "\tZ: " << bridge->endpoint1.z;
cout << endl;
lateralPosition bridgePos;
targetDistance pos;
if ( bridge->getInterpolatedPosition(&bridgePos, 0) ){
bridge->getTargetDistance(&bridgePos, &pos);
cout << "Dis Q21T1 X: " << bridge->disToQuad1.x << "\tY: " << bridge->disToQuad1.y << "\tZ: " << bridge->disToQuad1.z;
cout << endl;
}
}
if(bridge->endpoint2Valid){
cout << "Target 2 X: " << bridge->endpoint2.x << "\tY: " << bridge->endpoint2.y << "\tZ: " << bridge->endpoint2.z;
cout << endl;
}
#endif
// Print out the commands
cout << "Command List" << endl;
cout << "t = Takeoff" << endl;
......@@ -53,12 +36,12 @@ void* displayData(void* id) {
cout << "y# = Change Y Setpoint to # meters" << endl;
cout << "z# = Change Z Setpoint to # meters" << endl;
cout << "w# = Change Yaw setpoint to # degrees" << endl;
/* cout << "r# = Hover over bridge at # between targets" << endl;
cout << "r# = Hover over bridge at # between targets" << endl;
cout << "h = Land on bridge" << endl;
cout << "b = Takeoff from bridge" << endl;
cout << "k = Kill all motors" << endl;
cout << "m# = Send # to all motors (requires special firmware)" << endl;
*/ cout << "d = Goto default setpoint" << endl;
cout << "d = Goto default setpoint" << endl;
cout << endl;
// Print the headers
......@@ -227,6 +210,41 @@ void* displayData(void* id) {
}
cout << endl << endl;
#ifdef USE_BRIDGE
if(bridge->endpoint1Valid){
if(NUM_QUADS == 1){
bridge->getTargetDistance(&bridgePos, &pos1);
cout << setprecision(3) << "D T1 X: " << bridge->disToQuad1.x << endl;
cout << setprecision(3) << "D T1 Y: " << bridge->disToQuad1.y << endl;
cout << setprecision(3) << "D T1 Z: " << bridge->disToQuad1.z << endl;
cout << setprecision(3) << "D T1: " << bridge->disToQuad1.dis << endl;
}
if(NUM_QUADS == 2){
bridge->getTargetDistance(&bridgePos, &pos1, &pos2);
cout << setprecision(3) << "D T1 X: " << bridge->disToQuad1.x << "\t\t" << bridge->disToQuad2.x << endl;
cout << setprecision(3) << "D T1 Y: " << bridge->disToQuad1.y << "\t\t" << bridge->disToQuad2.y << endl;
cout << setprecision(3) << "D T1 Z: " << bridge->disToQuad1.z << "\t\t" << bridge->disToQuad2.z << endl;
cout << setprecision(3) << "D T1: " << bridge->disToQuad1.dis << "\t\t" << bridge->disToQuad2.dis << endl;
}
if(NUM_QUADS == 3){
bridge->getTargetDistance(&bridgePos, &pos1, &pos2, &pos3);
cout << setprecision(3) << "D T1 X: " << bridge->disToQuad1.x << "\t\t" << bridge->disToQuad2.x << "\t\t" << bridge->disToQuad3.x << endl;
cout << setprecision(3) << "D T1 Y: " << bridge->disToQuad1.y << "\t\t" << bridge->disToQuad2.y << "\t\t" << bridge->disToQuad3.y << endl;
cout << setprecision(3) << "D T1 Z: " << bridge->disToQuad1.z << "\t\t" << bridge->disToQuad2.z << "\t\t" << bridge->disToQuad3.z << endl;
cout << setprecision(3) << "D T1: " << bridge->disToQuad1.dis << "\t\t" << bridge->disToQuad2.dis << "\t\t" << bridge->disToQuad3.dis << endl;
}
if(NUM_QUADS == 4){
bridge->getTargetDistance(&bridgePos, &pos1, &pos2, &pos3, &pos4);
cout << setprecision(3) << "D T1 X: " << bridge->disToQuad1.x << "\t\t" << bridge->disToQuad2.x << "\t\t" << bridge->disToQuad3.x << "\t\t" << bridge->disToQuad4.x << endl;
cout << setprecision(3) << "D T1 Y: " << bridge->disToQuad1.y << "\t\t" << bridge->disToQuad2.y << "\t\t" << bridge->disToQuad3.y << "\t\t" << bridge->disToQuad4.y << endl;
cout << setprecision(3) << "D T1 Z: " << bridge->disToQuad1.z << "\t\t" << bridge->disToQuad2.z << "\t\t" << bridge->disToQuad3.z << "\t\t" << bridge->disToQuad4.z << endl;
cout << setprecision(3) << "D T1: " << bridge->disToQuad1.dis << "\t\t" << bridge->disToQuad2.dis << "\t\t" << bridge->disToQuad3.dis << "\t\t" << bridge->disToQuad4.dis << endl;
}
}
cout << endl;
#endif
cout << "Gyro X\t";
for (int i = 0; i < NUM_QUADS; i++) {
printf("%1.3f", crazyflie_info[i].cflieCopter->sensorDoubleValue("gyro.x"));
......@@ -282,6 +300,11 @@ void* displayData(void* id) {
printf("%1.2f", crazyflie_info[i].cflieCopter->sensorDoubleValue("pm.vbat"));
cout << "\t\t";
}
cout << endl << endl;
if(bridge->endpoint1Valid){
cout << "Target 1 X: " << bridge->endpoint1.x << "\tY: " << bridge->endpoint1.y << "\tZ: " << bridge->endpoint1.z;
cout << endl;
}
cout << endl << endl;
// Print the loop time
......@@ -298,7 +321,7 @@ void* displayData(void* id) {
return(NULL);
}
usleep(10000);
usleep(200000);
}
return(NULL);
......
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