Commit f0329052 authored by Ian McInerney's avatar Ian McInerney

Modified code to use the Bridge class

parent a9884684
......@@ -14,8 +14,12 @@
#include <quat.h>
#include "Bridge.h"
extern uint8_t exitProgram;
extern Bridge *bridge;
// The lowest battery level to fly with
#define LOW_BATTERY_LEVEL 3.2
......
......@@ -5,6 +5,7 @@
#include "userInput.h"
#include "userOutput.h"
#include "logData.h"
#include "Bridge.h"
#include "quadcopterData.h"
#include <sys/socket.h>
......@@ -21,6 +22,9 @@ const std::string erisIP = "192.168.0.196"; //TODO Change to IP of Other Linux C
const int port = 60551;
const int buffLen = 8;
// The bridge object
Bridge *bridge;
using namespace std;
void initMulticast();
......@@ -255,6 +259,8 @@ int main(int argc, char **argv) {
// crazyflie_info[i].cflieCopter->addLoggingEntry("position", "posCtlAlt.outz");
}
bridge = new Bridge();
// Initialize the VRPN connections
#if USE_HAND
vrpn_init("192.168.0.120:3883", handle_hand);
......
......@@ -124,9 +124,6 @@ void parseCommand() {
// Initialize variables outside the switch so the compiler is happy
float payload = 0;
float landingX = 0;
float landingY = 0;
float landingZ = 0;
// Per quadcopter commands
switch (tempCmd[1]) {
......@@ -211,17 +208,18 @@ void parseCommand() {
// Determine where on the bridge to land
payload = atof(&cmd[2]);
COMPUTE_BRIDGE_COORDINATE(payload, landingX, landingY, landingZ);
// Set the hover point
SET_X(quadcopterNumber, landingX);
SET_Y(quadcopterNumber, landingY);
SET_Z(quadcopterNumber, landingZ - 0.5);
crazyflie_info[quadcopterNumber-1].controllerData.bridgePos = payload;
SET_FLIGHTMODE(quadcopterNumber, BRIDGE_HOVER_MODE);
cout << "Crazyflie " << +quadcopterNumber << " Hover over bridge" << endl;
lateralPosition bridgePos;
if ( bridge->getInterpolatedPosition(&bridgePos, payload) ) {
// The position is valid
SET_X(quadcopterNumber, bridgePos.x);
SET_Y(quadcopterNumber, bridgePos.y);
SET_Z(quadcopterNumber, bridgePos.z - 0.5);
SET_FLIGHTMODE(quadcopterNumber, BRIDGE_HOVER_MODE);
cout << "Crazyflie " << +quadcopterNumber << " Hover over bridge" << endl;
} else {
cout << "Bridge is not valid" << endl;
}
break;
case CMD_BRIDGE_LAND:
......
......@@ -66,10 +66,10 @@ void vrpn_init(std::string connectionName, void (*callbackHand)(void*, const vrp
#ifdef USE_BRIDGE
trackerBridge1 = new vrpn_Tracker_Remote("Bridge1", connection);
trackerBridge1->register_change_handler(0, callbackBridge1);
trackerBridge1->register_change_handler(bridge, Bridge::end1Callback);
trackerBridge2 = new vrpn_Tracker_Remote("Bridge2", connection);
trackerBridge2->register_change_handler(0, callbackBridge2);
trackerBridge2->register_change_handler(bridge, Bridge::end2Callback);
#endif
// Initialize all the quadcopter trackables
......
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