Commit 1b0887b4 authored by ucart's avatar ucart

Updated Bridge code to also calculate distance from a quad to a bridge

parent 8231dfed
......@@ -8,15 +8,20 @@ class Bridge {
private:
public:
// Location of the two endpoints
lateralPosition endpoint1;
lateralPosition endpoint2;
targetDistance disToQuad1;
targetDistance disToQuad2;
targetDistance disToQuad3;
targetDistance disToQuad4;
bool endpoint1Valid;
bool endpoint2Valid;
public:
Bridge();
~Bridge();
......@@ -31,6 +36,15 @@ class Bridge {
*/
bool getInterpolatedPosition(lateralPosition *pos, int loc);
/**
* 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
* @return True if a location is valid, false if no location is able to found
*/
bool getDistanceToQuad1(lateralPosition *pos, targetDistance *disToQuad1);
/**
* Callbacks for the VRPN trackers on either end of the bridge
*/
......@@ -39,4 +53,4 @@ class Bridge {
};
#endif
\ No newline at end of file
#endif
......@@ -24,7 +24,7 @@ extern Bridge *bridge;
// The lowest battery level to fly with
#define LOW_BATTERY_LEVEL 3.2
//#define USE_BRIDGE 1
#define USE_BRIDGE 1
#define USE_LOCALIZATION
......
......@@ -128,6 +128,12 @@ typedef struct lateralPosition {
float z;
} lateralPosition;
typedef struct targetDistance {
float x;
float y;
float z;
} targetDistance;
typedef struct quaternionPosition {
float q0;
float q1;
......@@ -141,4 +147,4 @@ typedef struct takeoffProfileStep {
double condition;
} takeoffProfileStep;
#endif
\ No newline at end of file
#endif
......@@ -2,6 +2,8 @@
#include <vrpn_Tracker.h>
#include "structures.h"
#include "quadcopterData.h"
#include "crazyflieGroundStation.h"
Bridge::Bridge() {
// The endpoints are not valid yet
......@@ -50,6 +52,25 @@ bool Bridge::getInterpolatedPosition(lateralPosition *pos, int loc) {
return( retVal );
}
/**
* 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
* @return True if a location is valid, false if no location is able to found
*/
bool getDistanceToQuad1(lateralPosition *pos, targetDistance *disToQuad1) {
bool retVal = false;
if (bridge->endpoint1Valid){
coordinateSystem coor = crazyflie_info[1].cflieCopter->getCoordinateSystem();
disToQuad1->x = bridge->endpoint1.x - crazyflie_info[1].cflieCopter->getPositionX( coor );
disToQuad1->y = bridge->endpoint1.y - crazyflie_info[1].cflieCopter->getPositionY( coor );
disToQuad1->z = bridge->endpoint1.z - crazyflie_info[1].cflieCopter->getPositionZ( coor );
retVal = true;
}
return( retVal );
}
/**
* Callbacks for the VRPN trackers on either end of the bridge
......@@ -76,4 +97,4 @@ void VRPN_CALLBACK Bridge::end2Callback(void *inst, const vrpn_TRACKERCB t) {
// Say the endpoint is valid
bridge->endpoint2Valid = true;
}
\ No newline at end of file
}
......@@ -156,10 +156,10 @@ QUADCOPTERS_t crazyflie_info[] = {
},
*/
// Quadcopter 1
{"Crazyflie23",
{"Crazyflie21",
NULL,
NULL,
45,
25,
0,
XFER_2M,
0,
......@@ -195,7 +195,7 @@ QUADCOPTERS_t crazyflie_info[] = {
{0.0, 0.0, 0.0, 20.0, PID_X},
{0.0, 0.0, 0.0, 20.0, PID_Y},
{0.0, 0.0, 0.0, 20.0, PID_Z},
{Kp/Kd, Ki/Kd, 0.0, 20.0, PID_ROLL}, //*(180/3.14)
{Kp/Kd, Ki/Kd, 0.0, 20.0, PID_ROLL}, //(180/3.14)
{Kp/Kd, Ki/Kd, 0.0, 20.0, PID_PITCH}, //(6.0/7.0)
{0.0, 0.0, 0.0, 20.0, PID_YAW},
{-Kd, 0.0, 0.0, 20.0, PID_ROLL_RATE},
......@@ -276,7 +276,7 @@ QUADCOPTERS_t crazyflie_info[] = {
{"Crazyflie23",
NULL,
NULL,
85, //swapped radio on 1 and 3
65, //swapped radio on 1 and 3
1,
XFER_2M, // The datarate of the Crazyflie
0,
......@@ -320,7 +320,7 @@ QUADCOPTERS_t crazyflie_info[] = {
{"Crazyflie24",
NULL,
NULL,
65,
85,
1,
XFER_2M, // The datarate of the Crazyflie
0,
......
......@@ -3,6 +3,7 @@
#include "quadcopterData.h"
#include "callbacks.h"
#include "crazyflieGroundStation.h"
#include "Bridge.h"
#include <iostream>
using namespace std;
......@@ -19,9 +20,25 @@ void* displayData(void* id) {
printf("Spawning User Output Thread\n");
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;
cout << "Dis Q21T1 X: " << bridge->disToQuad1.x << "\tY: " << bridge->disToQuad1.y << "\tZ: " << bridge->disToQuad1.z;
}
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;
......@@ -30,12 +47,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
......@@ -74,6 +91,7 @@ void* displayData(void* id) {
case KILL_MODE:
cout << "Kill";
break;
/*
case BRIDGE_HOVER_MODE:
cout << "Bridge H";
break;
......@@ -83,6 +101,7 @@ void* displayData(void* id) {
case BRIDGE_TAKEOFF_MODE:
cout << "Bridge T";
break;
*/
case IDLE_MODE:
cout << "Idle";
break;
......@@ -111,7 +130,7 @@ void* displayData(void* id) {
coordinateSystem coor = crazyflie_info[i].cflieCopter->getCoordinateSystem();
printf("%1.3f", crazyflie_info[i].cflieCopter->getPositionX( coor ) );
cout << "\t\t";
}
}
cout << endl;
// Print the X setpoint
......@@ -267,15 +286,6 @@ void* displayData(void* id) {
}
cout << endl << endl;
/*
// Print the bridge
cout << "Bridge 1 X: " << bridge1.x << "\tY: " << bridge1.y << "\tZ: " << bridge1.z;
cout << endl;
// Print the bridge
cout << "Bridge 2 X: " << bridge2.x << "\tY: " << bridge2.y << "\tZ: " << bridge2.z;
cout << endl;
*/
// Check if the program should terminate
if (exitProgram) {
std::cout << "Stopping thread for user output" << endl;
......@@ -286,4 +296,4 @@ void* displayData(void* id) {
}
return(NULL);
}
\ No newline at end of file
}
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