Commit 42df93b5 authored by Ian McInerney's avatar Ian McInerney

Working refactored codebase

parent 1a815f34
......@@ -144,15 +144,7 @@ class CCrazyflie {
float m_startTime;
float xPosition;
float yPosition;
float zPosition;
float yawPosition;
float xSetpoint;
float ySetpoint;
float zSetpoint;
float yawSetpoint;
uint8_t resetController;
bool m_resetController;
// File stream for the console output
std::ofstream file_console;
......@@ -180,7 +172,6 @@ class CCrazyflie {
int m_nMinThrust;
double m_dSendSetpointPeriod;
double m_dSetpointLastSent;
bool m_bSendsSetpoints;
CTOC *m_tocParameters;
CTOC *m_tocLogs;
enum State m_enumState;
......@@ -188,6 +179,10 @@ class CCrazyflie {
// Function to hold the received logging packets in
std::list<CCRTPPacket*> m_lstLoggingPackets;
// Flags to say whether to send packets
bool m_sendSetpoints;
bool m_sendPosition;
// Functions
bool readTOCParameters();
bool readTOCLogs();
......@@ -298,6 +293,11 @@ class CCrazyflie {
bool enableThrustController();
bool disableThrustController();
/**
* Reset the controller on the quadcopter
*/
void resetController();
/**
* This function will set the flight mode of the Crazyflie
*/
......@@ -359,36 +359,17 @@ class CCrazyflie {
copter communication. */
bool isInitialized();
/*! \brief Set whether setpoints are currently sent while cycle()
While performing the cycle() function, the currently set setpoint
is sent to the copter regularly. If this is not the case, dummy
packets are sent. Using this mechanism, you can effectively switch
off sending new commands to the copter.
Default value: `false`
\param bSendSetpoints When set to `true`, the current setpoint is
sent while cycle(). Otherwise, not. */
void setSendSetpoints(bool bSendSetpoints);
/*! \brief Whether or not setpoints are currently sent to the copter
\return Boolean value denoting whether or not the current setpoint
is sent to the copter while performing cycle(). */
bool sendsSetpoints();
uint8_t setPID_Constants( PID_ControllerParams params );
bool sendPID_Constants( PID_ControllerParams params );
/*
void setXYZYawPosition(float xPosition, float yPosition, float zPosition,
float yawPosition);
void setXYZYawSetpoint(float xSetpoint, float ySetpoint, float zSetpoint,
float yawSetpoint, uint16_t baseThrust,
uint8_t resetController);
*/
/*
* Send setpoints for the quadcopter
*/
......
......@@ -278,7 +278,7 @@ void VRPN_CALLBACK handle_pos(void*, const vrpn_TRACKERCB t) {
/**
* The VRPN callback for the 2nd quadcopter
*/
void VRPN_CALLBACK handle_Crazyflie3(void*, const vrpn_TRACKERCB t) {
/*void VRPN_CALLBACK handle_Crazyflie3(void*, const vrpn_TRACKERCB t) {
// Pull out the X, Y, Z, Yaw position of the quadcopter
EXTRACT_POSITION(2, t);
......@@ -332,11 +332,11 @@ void VRPN_CALLBACK handle_Crazyflie3(void*, const vrpn_TRACKERCB t) {
// Do the main control loop
processTrackableCallback(2, crazyflie_info[2].radioNumber, &(crazyflie_info[2].controllerData), crazyflie_info[2].cflieCopter, t);
}
*/
/**
* The VRPN callback for the 2nd quadcopter
*/
void VRPN_CALLBACK handle_Crazyflie4(void*, const vrpn_TRACKERCB t) {
/*void VRPN_CALLBACK handle_Crazyflie4(void*, const vrpn_TRACKERCB t) {
// Pull out the X, Y, Z, Yaw position of the quadcopter
EXTRACT_POSITION(3, t);
......@@ -391,7 +391,7 @@ void VRPN_CALLBACK handle_Crazyflie4(void*, const vrpn_TRACKERCB t) {
processTrackableCallback(3, crazyflie_info[3].radioNumber, &(crazyflie_info[3].controllerData), crazyflie_info[3].cflieCopter, t);
}
*/
#if USE_HAND
/**
* VRPN callback for the hand trackable
......@@ -458,7 +458,7 @@ void VRPN_CALLBACK handle_hand(void*, const vrpn_TRACKERCB t) {
* @param controllerData The data structure containing information about the controller
* @param vrpn_TRACKERCB The VRPN data structure
*/
void processTrackableCallback(int crazyflieNum, int radioNum,
/*void processTrackableCallback(int crazyflieNum, int radioNum,
CONTROLLER_DATA_t *controllerData,
CCrazyflie *cflieCopter,
const vrpn_TRACKERCB t) {
......@@ -524,7 +524,7 @@ void processTrackableCallback(int crazyflieNum, int radioNum,
} else if (cflieCopter->pitch() < -90) {
thrustControlOutput = 10000;
}
*/
*//*
#if USE_PRINTOUT
if (cflieCopter->m_enumFlightMode != GROUNDED_MODE) {
cout << "Roll: " << cflieCopter->roll() << endl;
......@@ -612,4 +612,4 @@ void processTrackableCallback(int crazyflieNum, int radioNum,
controllerData->vrpnPacketQueueLen = 0;
return;
}
}
\ No newline at end of file
}*/
\ No newline at end of file
......@@ -48,7 +48,8 @@ CCrazyflie::CCrazyflie(CCrazyRadio *crRadio, int nRadioChannel, int quadNum, dou
m_fYaw = 0;
m_nThrust = 0;
m_bSendsSetpoints = false;
m_sendSetpoints = false;
m_sendPosition = false;
m_tocParameters = new CTOC(m_crRadio, this, nRadioChannel, 2);
m_tocLogs = new CTOC(m_crRadio, this, nRadioChannel, 5);
......@@ -185,11 +186,24 @@ bool CCrazyflie::cycle() {
break;
case STATE_NORMAL_OPERATION: {
if (m_enumFlightMode == TAKEOFF_MODE) {
this->takeoffCycle();
}
if( ALL_THE_DEBUG ) printf( " STATE_NORMAL_OPERATION\n" );
switch (this->m_enumFlightMode) {
case TAKEOFF_MODE:
// If the quad is taking off, run the takeoff routine
this->takeoffCycle();
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;
this->setFlightMode(GROUNDED_MODE);
}
break;
default:
break;
}
if (m_bufferEnd > 0) {
// Send PID constants
for (int i = 0; i < m_bufferEnd; i++) {
......@@ -199,14 +213,22 @@ bool CCrazyflie::cycle() {
// The buffer is empty
m_bufferEnd = 0;
}
if (m_bSendsSetpoints) {
// Send the current set point based on the previous calculations
//this->sendSetpoint(m_fRoll, m_fPitch, m_fYaw, m_nThrust);
// If the setpoints are new, send them
if (this->m_sendSetpoints) {
this->sendPositionSetpoint();
this->m_sendSetpoints = false;
// Reset the flag to reset the controllers
this->m_resetController = false;
}
// If the position is new, send it
if (this->m_sendPosition) {
this->sendCameraData();
m_bSendsSetpoints = false;
this->m_sendPosition = false;
} else {
// Send a dummy packet for keepalive
// Send a dummy packet for keepalive otherwise
m_crRadio->sendDummyPacket(m_nRadioChannel, this);
}
}
......@@ -250,18 +272,6 @@ bool CCrazyflie::isInitialized() {
return m_enumState == STATE_NORMAL_OPERATION;
}
void CCrazyflie::setSendSetpoints(bool bSendSetpoints) {
m_bSendsSetpoints = bSendSetpoints;
}
bool CCrazyflie::sendsSetpoints() {
return m_bSendsSetpoints;
}
enum State CCrazyflie::getCurrentState() {
return this->m_enumState;
}
//void CCrazyflie::setFlightMode(enum FLIGHT_MODE){
// m_enumFlightMode = FLIGHT_MODE;
//}
......@@ -151,6 +151,13 @@ bool CCrazyflie::sendCameraData() {
bool CCrazyflie::sendPositionSetpoint() {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
uint8_t resetValue;
if (this->m_resetController) {
resetValue = 1;
} else {
resetValue = 0;
}
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));
......@@ -158,7 +165,7 @@ bool CCrazyflie::sendPositionSetpoint() {
memcpy(&cBuffer[2 * sizeof(float)], &(this->m_desiredPosition.z), 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)], &resetController, sizeof(uint8_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,
......
......@@ -41,12 +41,12 @@ void CCrazyflie::takeoffCycle() {
this->enableThrustController();
// If it is the final step, then put it into hover mode
this->m_enumFlightMode = HOVER_MODE;
this->setFlightMode(HOVER_MODE);
}
// Set the base thrust and send the setpoints
this->m_baseThrust = this->takeoffProfile[this->currentStep].thrust;
this->m_bSendsSetpoints = true;
this->m_sendSetpoints = true;
}
/**
......@@ -62,6 +62,17 @@ bool CCrazyflie::enableThrustController() {
return( retVal );
}
/**
* Reset the controller on the quadcopter
*/
void CCrazyflie::resetController() {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
this->m_resetController = true;
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
}
/**
* Disable the thrust controller.
*/
......
......@@ -27,6 +27,24 @@ void CCrazyflie::setTakeoffProfile( takeoffProfileStep *profile, int numSteps )
*/
void CCrazyflie::setFlightMode( enum FlightMode mode) {
this->m_enumFlightMode = mode;
// Configure certain aspects when a mode is entered
switch (mode) {
case TAKEOFF_MODE:
this->currentStep = 0;
break;
case GROUNDED_MODE:
this->m_baseThrust = 0;
this->m_sendSetpoints = true;
break;
case LANDING_MODE:
this->m_desiredPosition.z = 0;
this->m_sendSetpoints = true;
break;
default:
// Do nothing
break;
}
}
/**
......@@ -34,20 +52,24 @@ void CCrazyflie::setFlightMode( enum FlightMode mode) {
*/
void CCrazyflie::setSetpointX(float setpoint) {
this->m_desiredPosition.x = setpoint;
this->m_sendSetpoints = true;
}
void CCrazyflie::setSetpointY(float setpoint) {
this->m_desiredPosition.y = setpoint;
this->m_sendSetpoints = true;
}
void CCrazyflie::setSetpointZ(float setpoint) {
this->m_desiredPosition.z = setpoint;
this->m_sendSetpoints = true;
}
void CCrazyflie::setSetpointYaw(float setpoint) {
this->m_desiredYaw = setpoint;
this->m_sendSetpoints = true;
}
/*
void CCrazyflie::setXYZYawSetpoint(float xSetpoint, float ySetpoint, float zSetpoint,
float yawSetpoint, uint16_t baseThrust,
uint8_t resetController) {
......@@ -60,6 +82,8 @@ void CCrazyflie::setXYZYawSetpoint(float xSetpoint, float ySetpoint, float zSetp
this->m_baseThrust = baseThrust;
this->resetController = resetController;
this->m_sendSetpoints = true;
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
}
......@@ -74,14 +98,16 @@ void CCrazyflie::setXYZYawPosition(float xPosition, float yPosition, float zPosi
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
}
*/
void CCrazyflie::setCameraPosition(lateralPosition cameraPosition) {
this->m_currentCameraPosition = cameraPosition;
this->m_sendPosition = true;
}
void CCrazyflie::setCameraAngle(angularPosition cameraAngle) {
this->m_currentCameraAngle = cameraAngle;
this->m_sendPosition = true;
}
void CCrazyflie::setCameraQuat(quaternionPosition cameraQuat) {
......
......@@ -265,8 +265,8 @@ int main(int argc, char **argv) {
#if USE_HAND
vrpn_init("192.168.0.120:3883", handle_hand);
#else
// vrpn_init("192.168.0.120:3883", NULL);
vrpn_init("localhost:3883", NULL);
vrpn_init("192.168.0.120:3883", NULL);
// vrpn_init("localhost:3883", NULL);
#endif // END USE_HAND
usleep(10000);
......@@ -315,7 +315,7 @@ int main(int argc, char **argv) {
pthread_mutex_init(&mutex, NULL);
pthread_create(&threads[0], NULL, UIThread, (void*)1);
pthread_create(&threads[1], NULL, vrpn_go, (void*)2);
pthread_create(&threads[2], NULL, displayData, (void*)3);
// pthread_create(&threads[2], NULL, displayData, (void*)3);
pthread_join(threads[1], &status);
// Run the VRPN main loop
......
......@@ -10,8 +10,8 @@
// Third column is the condition to end the step
takeoffProfileStep quad1_takeoffSteps[] = {
{19000, step_time, 1},
// {52000, step_height, -0.3},
{52000, step_time, 1},
{52000, step_height, -0.3},
// {52000, step_time, 1},
{42000, step_final, 0}
};
......@@ -112,16 +112,16 @@ QUADCOPTERS_t crazyflie_info[] = {
},
*/
// Quadcopter 1
{"Crazyflie2",
{"Crazyflie22",
//"Bridge1",
//"UAV",
// "BigQuad7",
NULL,
NULL,
// 75,
100,//swapped 1,2
// 80,
// 100,//swapped 1,2
// 80,
45,
0,
0,
// The controller data structure
......
......@@ -137,13 +137,14 @@ void parseCommand() {
case CMD_TAKEOFF:
// Configure the setpoints
SET_X(quadcopterNumber, crazyflie_info[quadcopterNumber-1].controllerData.xPosition);
SET_Y(quadcopterNumber, crazyflie_info[quadcopterNumber-1].controllerData.yPosition);
SET_X(quadcopterNumber, crazyflie_info[quadcopterNumber-1].cflieCopter->getCameraX());
SET_Y(quadcopterNumber, crazyflie_info[quadcopterNumber-1].cflieCopter->getCameraY());
SET_Z(quadcopterNumber, defaultSetpoints[quadcopterNumber-1][2]);
// Get the quad ready to takeoff
crazyflie_info[quadcopterNumber-1].cflieCopter->setTakeoffProfile( quad1_takeoffSteps, 3 );
crazyflie_info[quadcopterNumber-1].cflieCopter->disableThrustController();
crazyflie_info[quadcopterNumber-1].cflieCopter->resetController();
// Actually takeoff
SET_FLIGHTMODE(quadcopterNumber, TAKEOFF_MODE);
......
......@@ -110,7 +110,7 @@ void* vrpn_go(void *threadID) {
crazyflie_info[j].cflieCopter->cycle();
}
nearGround(); // See if the quadcopters have landed yet
// nearGround(); // See if the quadcopters have landed yet
#if USE_HAND
updateHand();
#endif // END USE_HAND
......
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