Commit d2f80e12 authored by Ian McInerney's avatar Ian McInerney

Added local reference frame for each Crazyflie

parent ad5418f7
......@@ -151,9 +151,18 @@ class CCrazyflie {
lateralPosition m_currentCameraPosition;
angularPosition m_currentCameraAngle;
quaternionPosition m_currentCameraQuat;
lateralPosition m_currentLocalPosition;
// The origin and limits of the local coordinate system
lateralPosition m_localOrigin;
limits m_localXlim;
limits m_localYlim;
limits m_localZlim;
coordinateSystem m_systemToUse;
// Structures to hold the desired setpoints
lateralPosition m_desiredPosition;
lateralPosition m_desiredPositionLocal;
lateralPosition m_desiredPositionGlobal;
float m_desiredYaw;
uint16_t m_baseThrust;
......@@ -326,9 +335,9 @@ class CCrazyflie {
* These functions will return the current quadcopter
* lateral position as reported by the camera system.
*/
float getCameraX();
float getCameraY();
float getCameraZ();
float getPositionX(coordinateSystem coor);
float getPositionY(coordinateSystem coor);
float getPositionZ(coordinateSystem coor);
/*
* These functions will return the current quadcopter
......@@ -349,9 +358,9 @@ class CCrazyflie {
/*
* These functions will return the current controller setpoints.
*/
float getSetpointX();
float getSetpointY();
float getSetpointZ();
float getSetpointX(coordinateSystem coor);
float getSetpointY(coordinateSystem coor);
float getSetpointZ(coordinateSystem coor);
float getSetpointYaw();
uint16_t getBaseThrust();
......@@ -361,12 +370,16 @@ class CCrazyflie {
/**
* These functions will set the setpoints of the quadcopter
*/
void setSetpointX(float setpoint);
void setSetpointY(float setpoint);
void setSetpointZ(float setpoint);
void setSetpointX(float setpoint, coordinateSystem coor);
void setSetpointY(float setpoint, coordinateSystem coor);
void setSetpointZ(float setpoint, coordinateSystem coor);
void setSetpointYaw(float setpoint);
void setBaseThrust(uint16_t thrust);
void setLocalOrigin(lateralPosition newOrigin);
void setCoordinateSystem(coordinateSystem coor);
coordinateSystem getCoordinateSystem();
/**
* Reset the controller on the quadcopter
*/
......
......@@ -25,6 +25,11 @@ typedef enum XferRate {
XFER_2M = 2,
} XferRate;
typedef enum coordinateSystem {
coor_global = 0,
coor_local,
} coordinateSystem;
typedef struct userCommands {
char type;
float payload;
......@@ -44,6 +49,11 @@ struct PID_ControllerParams {
uint8_t controllerID;
};
typedef struct limits {
float min;
float max;
} limits;
typedef struct angularPosition {
float roll;
float pitch;
......
......@@ -11,12 +11,13 @@ enum commandType {
CMD_SET_Z = 'z',
CMD_SET_YAW = 'w',
CMD_BRIDGE_HOVER = 'r',
CMD_BRIDGE_LAND = 'f',
CMD_BRIDGE_LAND = 'e',
CMD_BRIDGE_TAKEOFF = 'b',
CMD_KILL = 'k',
CMD_ALL_MOTORS = 'm',
CMD_GOTO_DEFAULT = 'd',
CMD_START_MOTORS = 's',
CMD_TOGGLE_COOR = 'f',
};
extern const std::string CLEAR; // When sent to cout, this clears the terminal screen
......
......@@ -82,13 +82,37 @@ CCrazyflie::CCrazyflie(CCrazyRadio *crRadio, int nRadioChannel, XferRate dataRat
m_currentCameraQuat.q1 = 0;
m_currentCameraQuat.q2 = 0;
m_currentCameraQuat.q3 = 1;
m_currentLocalPosition.x = 0;
m_currentLocalPosition.y = 0;
m_currentLocalPosition.z = 0;
// Initialize the setpoints
m_desiredPosition.x = 0;
m_desiredPosition.y = 0;
m_desiredPosition.z = 0;
m_desiredPositionGlobal.x = 0;
m_desiredPositionGlobal.y = 0;
m_desiredPositionGlobal.z = 0;
m_desiredYaw = 0;
m_baseThrust = 0;
m_desiredPositionLocal.x = 0;
m_desiredPositionLocal.y = 0;
m_desiredPositionLocal.z = 0;
// Default to the global coordinate system
m_systemToUse = coor_global;
// Default the local to be the same as the global, and some default extents
m_localOrigin.x = 0;
m_localOrigin.y = 0;
m_localOrigin.z = 0;
m_localXlim.min = -2;
m_localXlim.min = 2;
m_localYlim.min = -2;
m_localYlim.min = 2;
m_localZlim.min = -2.5;
m_localZlim.min = 0.5;
// Set the default flight mode to grounded
m_enumFlightMode = GROUNDED_MODE;
......@@ -228,8 +252,8 @@ bool CCrazyflie::cycle() {
break;
case LANDING_MODE:
// If the quad is landing, check for ground contact
if ( fabs(this->getCameraZ()) < 0.05) {
std::cout << "Low enough at: " << fabs(this->getCameraZ()) << std::endl;
if ( fabs(this->getPositionZ( coor_global )) < 0.05) {
std::cout << "Low enough at: " << fabs(this->getPositionZ( coor_global )) << std::endl;
this->setFlightMode(GROUNDED_MODE);
}
break;
......
......@@ -48,10 +48,12 @@ bool CCrazyflie::parseReceivedUserCommand() {
break;
case CMD_TAKEOFF:
this->setCoordinateSystem( coor_global );
// Configure the setpoints
this->setSetpointX( this->getCameraX() );
this->setSetpointY( this->getCameraY() );
this->setSetpointZ( defaultSetpoints[m_quadNum-1][2] );
this->setSetpointX( this->getPositionX( coor_global ), coor_global );
this->setSetpointY( this->getPositionY( coor_global ), coor_global );
this->setSetpointZ( defaultSetpoints[m_quadNum-1][2], coor_global );
// Get the quad ready to takeoff
this->setTakeoffProfile( quad1_takeoffSteps, quad1_numTakeoffSteps );
......@@ -64,9 +66,9 @@ bool CCrazyflie::parseReceivedUserCommand() {
break;
case CMD_GOTO_DEFAULT:
this->setSetpointX( defaultSetpoints[m_quadNum-1][0] );
this->setSetpointY( defaultSetpoints[m_quadNum-1][1] );
this->setSetpointZ( defaultSetpoints[m_quadNum-1][2] );
this->setSetpointX( defaultSetpoints[m_quadNum-1][0], coor_global );
this->setSetpointY( defaultSetpoints[m_quadNum-1][1], coor_global );
this->setSetpointZ( defaultSetpoints[m_quadNum-1][2], coor_global );
std::cout << "Crazyflie " << (m_quadNum+1) << " Going to default coordinates" << std::endl;
break;
......@@ -77,17 +79,17 @@ bool CCrazyflie::parseReceivedUserCommand() {
break;
case CMD_SET_X:
this->setSetpointX( command.payload );
this->setSetpointX( command.payload, this->m_systemToUse );
std::cout << "Crazyflie " << (m_quadNum+1) << " X Setpoint = " << command.payload << std::endl;
break;
case CMD_SET_Y:
this->setSetpointY( command.payload );
this->setSetpointY( command.payload, this->m_systemToUse );
std::cout << "Crazyflie " << (m_quadNum+1) << " Y Setpoint = " << command.payload << std::endl;
break;
case CMD_SET_Z:
this->setSetpointZ( command.payload );
this->setSetpointZ( command.payload, this->m_systemToUse );
std::cout << "Crazyflie " << (m_quadNum+1) << " Z Setpoint = " << command.payload << std::endl;
break;
......@@ -97,9 +99,9 @@ bool CCrazyflie::parseReceivedUserCommand() {
break;
case CMD_ALL_MOTORS:
this->setSetpointX( command.payload );
this->setSetpointY( command.payload );
this->setSetpointZ( command.payload );
this->setSetpointX( command.payload, coor_global );
this->setSetpointY( command.payload, coor_global );
this->setSetpointZ( command.payload, coor_global );
this->setSetpointYaw( command.payload );
std::cout << "Crazyflie " << (m_quadNum+1) << " Setting motors to = " << command.payload << std::endl;
break;
......@@ -109,9 +111,9 @@ bool CCrazyflie::parseReceivedUserCommand() {
lateralPosition bridgePos;
if ( bridge->getInterpolatedPosition(&bridgePos, command.payload) ) {
// The position is valid
this->setSetpointX( bridgePos.x);
this->setSetpointY( bridgePos.y);
this->setSetpointZ( bridgePos.z - 0.5);
this->setSetpointX( bridgePos.x, coor_global);
this->setSetpointY( bridgePos.y, coor_global);
this->setSetpointZ( bridgePos.z - 0.5, coor_global);
this->setFlightMode( BRIDGE_HOVER_MODE );
std::cout << "Crazyflie " << (m_quadNum+1) << " Hover over bridge" << std::endl;
} else {
......@@ -121,7 +123,7 @@ bool CCrazyflie::parseReceivedUserCommand() {
case CMD_BRIDGE_LAND:
// Land on the bridge by just changing the Z setpoint
this->setSetpointZ( this->getSetpointZ() + 0.7 );
this->setSetpointZ( this->getSetpointZ(this->m_systemToUse) + 0.7, this->m_systemToUse );
this->setFlightMode( BRIDGE_LANDING_MODE );
std::cout << "Crazyflie " << (m_quadNum+1) << " Land on bridge" << std::endl;
......@@ -131,6 +133,30 @@ bool CCrazyflie::parseReceivedUserCommand() {
this->setFlightMode( BRIDGE_TAKEOFF_MODE );
std::cout << "Crazyflie " << (m_quadNum+1) << " Take off from bridge" << std::endl;
break;
case CMD_TOGGLE_COOR:
// Toggle the coordinate system
if (this->m_systemToUse == coor_local) {
this->m_systemToUse = coor_global;
// Remap the setpoints so there is no jump
this->setSetpointX( this->getSetpointX(coor_local) + this->m_localOrigin.x , coor_global);
this->setSetpointY( this->getSetpointY(coor_local) + this->m_localOrigin.y , coor_global);
this->setSetpointZ( this->getSetpointZ(coor_local) + this->m_localOrigin.z , coor_global);
std::cout << "Crazyflie " << (m_quadNum+1) << " Using global system" << std::endl;
} else {
this->m_systemToUse = coor_local;
// Remap the setpoints so there is no jump
this->setSetpointX( this->getSetpointX(coor_global) - this->m_localOrigin.x , coor_local);
this->setSetpointY( this->getSetpointY(coor_global) - this->m_localOrigin.y , coor_local);
this->setSetpointZ( this->getSetpointZ(coor_global) - this->m_localOrigin.z , coor_local);
std::cout << "Crazyflie " << (m_quadNum+1) << "Using local system" << std::endl;
}
break;
}
// A command was parsed
......
......@@ -122,15 +122,19 @@ bool CCrazyflie::sendCameraData() {
int nSize = 4 * sizeof(float);
char cBuffer[nSize];
float xPosition = this->getPositionX(this->m_systemToUse);
float yPosition = this->getPositionY(this->m_systemToUse);
float zPosition = this->getPositionZ(this->m_systemToUse);
// Copy the data into a send buffer
memcpy(&cBuffer[0 * sizeof(float)], &(this->m_currentCameraPosition.x), sizeof(float));
memcpy(&cBuffer[1 * sizeof(float)], &(this->m_currentCameraPosition.y), sizeof(float));
memcpy(&cBuffer[2 * sizeof(float)], &(this->m_currentCameraPosition.z), sizeof(float));
memcpy(&cBuffer[0 * sizeof(float)], &(xPosition), sizeof(float));
memcpy(&cBuffer[1 * sizeof(float)], &(yPosition), sizeof(float));
memcpy(&cBuffer[2 * sizeof(float)], &(zPosition), sizeof(float));
memcpy(&cBuffer[3 * sizeof(float)], &(this->m_currentCameraAngle.yaw), sizeof(float));
if( ALL_THE_DEBUG ) printf( "sending position (%5.3f, %5.3f, %5.3f, %4.1f)\n",
this->m_currentCameraPosition.x, this->m_currentCameraPosition.y,
this->m_currentCameraPosition.z, this->m_currentCameraAngle.yaw);
xPosition, yPosition, zPosition, this->m_currentCameraAngle.yaw);
// The XYZ current position is on port 7
CCRTPPacket *crtpPacket = new CCRTPPacket(cBuffer, nSize, CRTP_PORT_XYZ_POS);
......@@ -158,18 +162,24 @@ bool CCrazyflie::sendPositionSetpoint() {
resetValue = 0;
}
float xSetpoint = this->getSetpointX(this->m_systemToUse);
float ySetpoint = this->getSetpointY(this->m_systemToUse);
float zSetpoint = this->getSetpointZ(this->m_systemToUse);
int nSize = 4 * sizeof(float) + 1*sizeof(uint16_t) + 1*sizeof(uint8_t);
char cBuffer[nSize];
memcpy(&cBuffer[0 * sizeof(float)], &(this->m_desiredPosition.x), sizeof(float));
memcpy(&cBuffer[1 * sizeof(float)], &(this->m_desiredPosition.y), sizeof(float));
memcpy(&cBuffer[2 * sizeof(float)], &(this->m_desiredPosition.z), sizeof(float));
memcpy(&cBuffer[0 * sizeof(float)], &(xSetpoint), sizeof(float));
memcpy(&cBuffer[1 * sizeof(float)], &(ySetpoint), sizeof(float));
memcpy(&cBuffer[2 * sizeof(float)], &(zSetpoint), sizeof(float));
memcpy(&cBuffer[3 * sizeof(float)], &(this->m_desiredYaw), sizeof(float));
memcpy(&cBuffer[4 * sizeof(float)], &(this->m_baseThrust), sizeof(uint16_t));
memcpy(&cBuffer[4 * sizeof(float) + sizeof(uint16_t)], &resetValue, sizeof(uint8_t));
if( ALL_THE_DEBUG ) printf( "sending setpoint (%5.3f, %5.3f, %5.3f, %4.1f)\n",
this->m_desiredPosition.x, this->m_desiredPosition.y,
this->m_desiredPosition.z, this->m_desiredYaw);
xSetpoint, ySetpoint, zSetpoint, this->m_desiredYaw);
// The XYZ setpoint is on port 6
CCRTPPacket *crtpPacket = new CCRTPPacket(cBuffer, nSize, CRTP_PORT_XYZ_SET);
......
......@@ -33,7 +33,7 @@ void CCrazyflie::takeoffCycle() {
break;
case step_height:
// The next step is height based
if ( this->getCameraZ() < this->takeoffProfile[this->currentStep].condition ) {
if ( this->getPositionZ( coor_global ) < this->takeoffProfile[this->currentStep].condition ) {
// The step has ended, move to the next one
this->currentStep++;
if (this->currentStep > this->numTakeoffSteps) {
......
......@@ -47,23 +47,44 @@ int CCrazyflie::getQuadcopterNumber() {
* These functions will return the current quadcopter
* lateral position as reported by the camera system.
*/
float CCrazyflie::getCameraX() {
float CCrazyflie::getPositionX(coordinateSystem coor) {
float retVal = 0;
pthread_mutex_lock( &(this->cameraDataMutex) );
float retVal = this->m_currentCameraPosition.x;
if (coor == coor_local) {
retVal = this->m_currentLocalPosition.x;
} else {
retVal = this->m_currentCameraPosition.x;
}
pthread_mutex_unlock( &(this->cameraDataMutex) );
return(retVal);
}
float CCrazyflie::getCameraY() {
float CCrazyflie::getPositionY(coordinateSystem coor) {
float retVal = 0;
pthread_mutex_lock( &(this->cameraDataMutex) );
float retVal = this->m_currentCameraPosition.y;
if (coor == coor_local) {
retVal = this->m_currentLocalPosition.y;
} else {
retVal = this->m_currentCameraPosition.y;
}
pthread_mutex_unlock( &(this->cameraDataMutex) );
return(retVal);
}
float CCrazyflie::getCameraZ() {
float CCrazyflie::getPositionZ(coordinateSystem coor) {
float retVal = 0;
pthread_mutex_lock( &(this->cameraDataMutex) );
float retVal = this->m_currentCameraPosition.z;
if (coor == coor_local) {
retVal = this->m_currentLocalPosition.z;
} else {
retVal = this->m_currentCameraPosition.z;
}
pthread_mutex_unlock( &(this->cameraDataMutex) );
return(retVal);
}
......@@ -115,16 +136,40 @@ float CCrazyflie::getCameraYaw() {
/*
* These functions will return the current controller setpoints.
*/
float CCrazyflie::getSetpointX() {
return this->m_desiredPosition.x;
float CCrazyflie::getSetpointX(coordinateSystem coor) {
float retVal = 0;
if (coor == coor_local) {
retVal = this->m_desiredPositionLocal.x;
} else {
retVal = this->m_desiredPositionGlobal.x;
}
return retVal;
}
float CCrazyflie::getSetpointY() {
return this->m_desiredPosition.y;
float CCrazyflie::getSetpointY(coordinateSystem coor) {
float retVal = 0;
if (coor == coor_local) {
retVal = this->m_desiredPositionLocal.y;
} else {
retVal = this->m_desiredPositionGlobal.y;
}
return retVal;
}
float CCrazyflie::getSetpointZ() {
return this->m_desiredPosition.z;
float CCrazyflie::getSetpointZ(coordinateSystem coor) {
float retVal = 0;
if (coor == coor_local) {
retVal = this->m_desiredPositionLocal.z;
} else {
retVal = this->m_desiredPositionGlobal.z;
}
return retVal;
}
float CCrazyflie::getSetpointYaw() {
......@@ -137,4 +182,8 @@ uint16_t CCrazyflie::getBaseThrust() {
double CCrazyflie::getPositionTimeDelta() {
return this->positionTimeDelta;
}
coordinateSystem CCrazyflie::getCoordinateSystem() {
return this->m_systemToUse;
}
\ No newline at end of file
......@@ -14,8 +14,13 @@
// Various headers for the files
static const char *variables = "%Time\t\tPositionDelta\t\tBattery\t\t"
"xSetpoint\t\tySetpoint\t\tzSetpoint\t\tyawSetpoint\t\t"
"xPosition\t\tyPosition\t\tzPosition\t\t"
"xSetpointGlobal\t\txSetpointLocal\t\t"
"ySetpointGlobal\t\tySetpointLocal\t\t"
"zSetpointGlobal\t\tzSetpointLocal\t\t"
"yawSetpoint\t\t"
"xPositionGlobal\t\txPositionLocal\t\t"
"yPositionGlobal\t\tyPositionLocal\t\t"
"zPositionGlobal\t\tzPositionLocal\t\t"
"quadPitch\t\tquadRoll\t\tquadYaw\t\t\t\tcamPitch\t\tcamRoll\t\tcamYaw\t\t"
"gyroX\t\tgyroY\t\tgyroZ\t\taccelX\t\taccelY\t\taccelZ\t\t"
"motor1\t\tmotor2\t\tmotor3\t\tmotor4\t\t"
......@@ -26,8 +31,13 @@ static const char *variables = "%Time\t\tPositionDelta\t\tBattery\t\t"
"Ut\t\tUa\t\tUe\t\tUr";
static const char *units = "&sec\t\tsec\t\tVolt\t\t"
"meters\t\tmeters\t\tmeters\t\tmeters\t\t"
"meters\t\tmeters\t\tmeters\t\t"
"meters\t\tmeters\t\t"
"meters\t\tmeters\t\t"
"meters\t\tmeters\t\t"
"degrees\t\t"
"meters\t\tmeters\t\t"
"meters\t\tmeters\t\t"
"meters\t\tmeters\t\t"
"degrees\t\tdegrees\t\tdegrees\t\tdegrees\t\tdegrees\t\tdegrees\t\t"
"deg\\sec\t\tdeg\\sec\t\tdeg\\sec\t\tg\t\tg\t\tg\t\t"
"raw\t\traw\t\traw\t\traw\t\t"
......@@ -91,15 +101,21 @@ void CCrazyflie::writeLogData() {
file_log << "\t\t" << this->batteryLevel();
// Print the setpoints
file_log << "\t\t" << this->getSetpointX();
file_log << "\t\t" << this->getSetpointY();
file_log << "\t\t" << this->getSetpointZ();
file_log << "\t\t" << this->getSetpointX(coor_global);
file_log << "\t\t" << this->getSetpointX(coor_local);
file_log << "\t\t" << this->getSetpointY(coor_global);
file_log << "\t\t" << this->getSetpointY(coor_local);
file_log << "\t\t" << this->getSetpointZ(coor_global);
file_log << "\t\t" << this->getSetpointY(coor_local);
file_log << "\t\t" << this->getSetpointYaw();
// Print the current position
file_log << "\t\t" << this->getCameraX();
file_log << "\t\t" << this->getCameraY();
file_log << "\t\t" << this->getCameraZ();
// Print the current local position
file_log << "\t\t" << this->getPositionX(coor_global);
file_log << "\t\t" << this->getPositionX(coor_local);
file_log << "\t\t" << this->getPositionY(coor_global);
file_log << "\t\t" << this->getPositionY(coor_local);
file_log << "\t\t" << this->getPositionZ(coor_global);
file_log << "\t\t" << this->getPositionZ(coor_local);
// Print the angles
file_log << "\t\t" << this->getQuadPitch();
......
......@@ -37,7 +37,8 @@ void CCrazyflie::setFlightMode( enum FlightMode mode) {
this->setBaseThrust( 0 );
break;
case LANDING_MODE:
this->setSetpointZ( 0 );
this->setCoordinateSystem( coor_global );
this->setSetpointZ( 0, coor_global );
break;
default:
// Do nothing
......@@ -48,18 +49,45 @@ void CCrazyflie::setFlightMode( enum FlightMode mode) {
/**
* These functions will set the setpoints of the quadcopter
*/
void CCrazyflie::setSetpointX(float setpoint) {
this->m_desiredPosition.x = setpoint;
void CCrazyflie::setSetpointX(float setpoint, coordinateSystem coor) {
if (coor == coor_local) {
// The given coordinate is in the local system
this->m_desiredPositionLocal.x = setpoint;
this->m_desiredPositionGlobal.x = setpoint + this->m_localOrigin.x;
} else {
//
this->m_desiredPositionGlobal.x = setpoint;
this->m_desiredPositionLocal.x = setpoint - this->m_localOrigin.x;
}
this->m_sendSetpoints = true;
}
void CCrazyflie::setSetpointY(float setpoint) {
this->m_desiredPosition.y = setpoint;
void CCrazyflie::setSetpointY(float setpoint, coordinateSystem coor) {
if (coor == coor_local) {
// The given coordinate is in the local system
this->m_desiredPositionLocal.y = setpoint;
this->m_desiredPositionGlobal.y = setpoint + this->m_localOrigin.y;
} else {
//
this->m_desiredPositionGlobal.y = setpoint;
this->m_desiredPositionLocal.y = setpoint - this->m_localOrigin.y;
}
this->m_sendSetpoints = true;
}
void CCrazyflie::setSetpointZ(float setpoint) {
this->m_desiredPosition.z = setpoint;
void CCrazyflie::setSetpointZ(float setpoint, coordinateSystem coor) {
if (coor == coor_local) {
// The given coordinate is in the local system
this->m_desiredPositionLocal.z = setpoint;
this->m_desiredPositionGlobal.z = setpoint + this->m_localOrigin.z;
} else {
//
this->m_desiredPositionGlobal.z = setpoint;
this->m_desiredPositionLocal.z = setpoint - this->m_localOrigin.z;
}
this->m_sendSetpoints = true;
}
......@@ -115,7 +143,14 @@ void CCrazyflie::setCameraPosition(lateralPosition cameraPosition) {
}
this->lastPositionTime = currentTime;
// Set the global position
this->m_currentCameraPosition = cameraPosition;
// Set the local position as well
this->m_currentLocalPosition.x = cameraPosition.x - this->m_localOrigin.x;
this->m_currentLocalPosition.y = cameraPosition.y - this->m_localOrigin.y;
this->m_currentLocalPosition.z = cameraPosition.z - this->m_localOrigin.z;
this->m_sendPosition = true;
pthread_mutex_unlock( &(this->cameraDataMutex) );
}
......@@ -135,4 +170,14 @@ void CCrazyflie::setCameraQuat(quaternionPosition cameraQuat) {
void CCrazyflie::setNetworkForwarding(NetworkForwarding *network) {
this->m_network = network;
}
void CCrazyflie::setLocalOrigin(lateralPosition newOrigin) {
this->m_localOrigin = newOrigin;
}
void CCrazyflie::setCoordinateSystem(coordinateSystem coor) {
this->m_systemToUse = coor;
this->m_sendSetpoints = true;
this->m_sendPosition = true;
}
\ No newline at end of file
......@@ -149,6 +149,12 @@ int main(int argc, char **argv) {
// Check the controller type
cout << crazyflie_info[i].cflieCopter->getControllerTypeString() << endl;
lateralPosition localOrigin;
localOrigin.x = -0.5;
localOrigin.y = 0.5;
localOrigin.z = -1;
crazyflie_info[i].cflieCopter->setLocalOrigin( localOrigin);
// Change the PID controller values
cout << "Sending PID Constants" << endl;
usleep(1000);
......@@ -269,7 +275,7 @@ int main(int argc, char **argv) {
#if USE_HAND
vrpn_init("192.168.0.120:3883", handle_hand);
#else
vrpn_init("192.168.0.194:3883", NULL);
vrpn_init("192.168.0.120:3883", NULL);
// vrpn_init("localhost:3883", NULL);
#endif // END USE_HAND
......
......@@ -180,8 +180,8 @@ QUADCOPTERS_t crazyflie_info[] = {
{Kd, 0.0, 0.0, 20.0, PID_PITCH_RATE},
{0.0, 0.0, 0.0, 20.0, PID_YAW_RATE}
*/
{-40.0/4, -2.0/4, -44.0/4, 40.0, PID_X},
{40.0/4, 2.0/4, 44.0/4, 40.0, PID_Y},
{-40.0/2, -2.0/2, -44.0/2, 40.0, PID_X},
{40.0/2, 2.0/2, 44.0/2, 40.0, PID_Y},
{-10000.0, -2000.0, -15000.0, 10000.0, PID_Z},
// {0.0, 0.0, 0.0, 40.0, PID_X},
// {0.0, 0.0, 0.0, 40.0, PID_Y},
......
......@@ -31,7 +31,7 @@ void* displayData(void* id) {
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 << "f = Land on bridge" << 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;
......@@ -93,10 +93,23 @@ void* displayData(void* id) {