Commit 15b97156 authored by Ian McInerney's avatar Ian McInerney

Made command parsing into functions inside the CCrazyflie class

parent f6733668
......@@ -42,6 +42,7 @@
#include <iostream>
#include <fstream>
#include <string.h>
#include <pthread.h>
// Private
#include "CCrazyRadio.h"
......@@ -181,6 +182,10 @@ class CCrazyflie {
// Various things for setting the PID parameters of the quadcopter
std::queue<PID_ControllerParams> pidToSend;
// Queue for the commands received from the user
std::queue<userCommands_t> userCommands;
pthread_mutex_t userCommandsMutex;
// Control related parameters
/*! \brief Maximum absolute value for the roll that will be sent to
the copter. */
......@@ -231,6 +236,14 @@ class CCrazyflie {
*/
bool modifyThrustController(bool enabled);
/**
* Parse the commands that are sitting in the queue.
* Each call will only parse one command.
*
* @return True if command parsed, false otherwise
*/
bool parseReceivedUserCommand();
public:
/*! \brief Constructor for the copter convenience class
......@@ -260,6 +273,13 @@ class CCrazyflie {
*/
void writeLogData();
/**
* Pass a command from the user into this Crazyflie
*
* @param command The command to pass in
*/
void passUserCommand(userCommands_t command);
/*
* Callback called whenever there is a new VRPN position packet.
* Note: This function is static in the class.
......
......@@ -3,6 +3,11 @@
#include <stdint.h>
typedef struct userCommands {
char type;
float payload;
} userCommands_t;
enum takeoffStepType {
step_final = 0,
step_time,
......
......@@ -18,20 +18,9 @@ enum commandType {
CMD_GOTO_DEFAULT = 'd',
CMD_START_MOTORS = 's',
};
/*
struct command {
uint8_t quadcopterNumber;
enum commandType commandType;
float payload1;
float payload2;
float payload3;
};
*/
extern pthread_mutex_t mutex; // Mutex for the threads
extern const std::string CLEAR; // When sent to cout, this clears the terminal screen
void* UIThread(void *threadID);
void parseCommand();
#endif
\ No newline at end of file
......@@ -29,6 +29,7 @@
#include <stdbool.h>
#include <iostream>
#include <fstream>
#include <pthread.h>
#include "CCrazyflie.h"
CCrazyflie::CCrazyflie(CCrazyRadio *crRadio, int nRadioChannel, int quadNum, double startTime) {
......@@ -98,6 +99,9 @@ CCrazyflie::CCrazyflie(CCrazyRadio *crRadio, int nRadioChannel, int quadNum, dou
m_startTime = startTime;
// Initialize the mutexes
pthread_mutex_init( &(this->userCommandsMutex), NULL);
// Open the file
try {
char fileName[255];
......@@ -230,12 +234,19 @@ bool CCrazyflie::cycle() {
// Place a timeout of 5 packets on the sending
int count = 0;
CCRTPPacket *pk = NULL;
if( m_network->retrievePacket(m_quadNum, pk) && (count < 5) ) {
while( m_network->retrievePacket(m_quadNum, pk) && (count < 5) ) {
this->m_crRadio->sendPacket( m_nRadioChannel, pk, this);
count++;
}
delete pk;
// If there are commands, process them
count = 0;
while ( this->parseReceivedUserCommand() && (count < 5) ) {
count++;
}
// If there are PID parameters, send them
if ( this->pidToSend.size() > 0 ) {
PID_ControllerParams param = this->pidToSend.front();
this->pidToSend.pop();
......
#include "CCrazyflie.h"
#include "structures.h"
#include "userInput.h"
#include "quadcopterData.h"
#include "crazyflieGroundStation.h"
/**
* Pass a command from the user into this Crazyflie
*
* @param command The command to pass in
*/
void CCrazyflie::passUserCommand(userCommands_t command) {
pthread_mutex_lock( &(this->userCommandsMutex) );
userCommands.push(command);
pthread_mutex_unlock( &(this->userCommandsMutex) );
}
/**
* Parse the commands that are sitting in the queue.
* Each call will only parse one command.
*
* @return True if command parsed, false otherwise
*/
bool CCrazyflie::parseReceivedUserCommand() {
// Get the mutex
pthread_mutex_lock( &(userCommandsMutex) );
// If there are no commands, then return
if (userCommands.size() == 0) {
pthread_mutex_unlock( &(userCommandsMutex) );
return (false);
}
// Get the next command from the queue and release the mutex
userCommands_t command = userCommands.front();
userCommands.pop();
pthread_mutex_unlock( &(userCommandsMutex) );
// Parse the command
switch ( command.type ) {
case CMD_KILL:
this->setFlightMode( GROUNDED_MODE );
std::cout << "Crazyflie " << (m_quadNum+1) << " Killed" << std::endl;
break;
case CMD_TAKEOFF:
// Configure the setpoints
this->setSetpointX( this->getCameraX() );
this->setSetpointY( this->getCameraY() );
this->setSetpointZ( defaultSetpoints[m_quadNum-1][2] );
// Get the quad ready to takeoff
this->setTakeoffProfile( quad1_takeoffSteps, quad1_numTakeoffSteps );
this->disableThrustController();
this->resetController();
// Actually takeoff
this->setFlightMode( TAKEOFF_MODE );
std::cout << "Crazyflie " << (m_quadNum+1) << " Taking off" << std::endl;
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] );
std::cout << "Crazyflie " << (m_quadNum+1) << " Going to default coordinates" << std::endl;
break;
case CMD_LAND:
this->setFlightMode( LANDING_MODE );
std::cout << "Crazyflie " << (m_quadNum+1) << " Landing" << std::endl;
break;
case CMD_SET_X:
this->setSetpointX( command.payload );
std::cout << "Crazyflie " << (m_quadNum+1) << " X Setpoint = " << command.payload << std::endl;
break;
case CMD_SET_Y:
this->setSetpointY( command.payload );
std::cout << "Crazyflie " << (m_quadNum+1) << " Y Setpoint = " << command.payload << std::endl;
break;
case CMD_SET_Z:
this->setSetpointZ( command.payload );
std::cout << "Crazyflie " << (m_quadNum+1) << " Z Setpoint = " << command.payload << std::endl;
break;
case CMD_SET_YAW:
this->setSetpointYaw( command.payload );
std::cout << "Crazyflie " << (m_quadNum+1) << " Yaw Setpoint = " << command.payload << std::endl;
break;
case CMD_ALL_MOTORS:
this->setSetpointX( command.payload );
this->setSetpointY( command.payload );
this->setSetpointZ( command.payload );
this->setSetpointYaw( command.payload );
std::cout << "Crazyflie " << (m_quadNum+1) << " Setting motors to = " << command.payload << std::endl;
break;
case CMD_BRIDGE_HOVER:
// Determine where on the bridge to land
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->setFlightMode( BRIDGE_HOVER_MODE );
std::cout << "Crazyflie " << (m_quadNum+1) << " Hover over bridge" << std::endl;
} else {
std::cout << "Bridge is not valid" << std::endl;
}
break;
case CMD_BRIDGE_LAND:
// Land on the bridge by just changing the Z setpoint
this->setSetpointZ( this->getSetpointZ() + 0.7 );
this->setFlightMode( BRIDGE_LANDING_MODE );
std::cout << "Crazyflie " << (m_quadNum+1) << " Land on bridge" << std::endl;
break;
case CMD_BRIDGE_TAKEOFF:
this->setFlightMode( BRIDGE_TAKEOFF_MODE );
std::cout << "Crazyflie " << (m_quadNum+1) << " Take off from bridge" << std::endl;
break;
}
// A command was parsed
return( true );
}
\ No newline at end of file
......@@ -48,6 +48,8 @@ void CCrazyflie::takeoffCycle() {
// If it is the final step, then put it into hover mode
this->setFlightMode(HOVER_MODE);
std::cout << "Takeoff complete" << std::endl;
}
// Set the base thrust
......
......@@ -309,7 +309,6 @@ int main(int argc, char **argv) {
void* status;
pthread_mutex_init(&mutex, NULL);
pthread_create(&threads[0], NULL, UIThread, (void*)1);
pthread_create(&threads[1], NULL, vrpn_go, (void*)2);
......
......@@ -16,9 +16,6 @@
using namespace std;
pthread_mutex_t mutex; // Mutex for the threads
static char cmd[255]; // Used in the UI thread for taking input
/**
* This thread will get user input
*/
......@@ -42,15 +39,45 @@ void* UIThread(void *threadID) {
// Loop forever while getting input
while (1) {
// Get a character from the terminal
tempbuf[counter] = getchar();
// If the characteris a newline, the command is entered
if (tempbuf[counter] == '\n') {
pthread_mutex_lock(&mutex);
// Add a null terminator to the buffer
tempbuf[counter] = 0;
strcpy(cmd, tempbuf);
pthread_mutex_unlock(&mutex);
// No command to parse
if ( counter == 0 ) {
break;
}
uint8_t commandLength = strlen(tempbuf);
if ( commandLength < 2) {
// Invalid command
counter = 0;
break;
}
// Get the quadcopter number
char temp[2];
temp[0] = tempbuf[0];
temp[1] = 0;
uint8_t quadcopterNumber = atoi( temp );
// Get the command type and the payload
userCommands_t command;
command.type = tempbuf[1];
command.payload = atof(&tempbuf[2]);
// Pass the command to the Crazyflie
crazyflie_info[quadcopterNumber-1].cflieCopter->passUserCommand(command);
// Print it out and then reset the counter
printf("\nReceived command: %s\n", tempbuf);
counter = 0;
} else {
counter++;
}
......@@ -63,177 +90,3 @@ void* UIThread(void *threadID) {
}
return NULL;
}
/**
* Parse the command from the user
*/
void parseCommand() {
// Copy the command into a temp variable for usage
char tempCmd[255];
pthread_mutex_lock(&mutex);
strcpy(tempCmd, cmd);
cmd[0] = 0;
pthread_mutex_unlock(&mutex);
// No command to parse
if ( tempCmd[0] == 0 ) {
return;
}
uint8_t commandLength = strlen(tempCmd);
// Invalid command
if ( commandLength < 2) {
tempCmd[0] = 0;
return;
}
// Get the quadcopter to modify
char temp[2];
temp[0] = tempCmd[0];
temp[1] = 0;
uint8_t quadcopterNumber = atoi( temp );
// Make sure the quadcopter exists
if ( !(quadcopterNumber <= NUM_QUADS) ) {
tempCmd[0] = 0;
cout << "Error, quadcopter does not exist" << endl;
return;
}
// Implement global commands
if (quadcopterNumber == 0) {
enum FlightMode mode;
switch (tempCmd[1]) {
case CMD_TAKEOFF:
mode = TAKEOFF_MODE;
cout << "All Crazyflies Taking off" << endl;
break;
case CMD_LAND:
mode = LANDING_MODE;
cout << "All Crazyflies Landing" << endl;
break;
}
for (int i = 0; i < NUM_QUADS; i++) {
crazyflie_info[i].cflieCopter->m_enumFlightMode = mode;
}
tempCmd[0] = 0;
return;
}
// Initialize variables outside the switch so the compiler is happy
float payload = 0;
// Per quadcopter commands
switch (tempCmd[1]) {
case CMD_KILL:
SET_FLIGHTMODE(quadcopterNumber, GROUNDED_MODE);
cout << "Crazyflie " << +quadcopterNumber << " Killed" << endl;
break;
case CMD_TAKEOFF:
// Configure the setpoints
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, quad1_numTakeoffSteps );
crazyflie_info[quadcopterNumber-1].cflieCopter->disableThrustController();
crazyflie_info[quadcopterNumber-1].cflieCopter->resetController();
// Actually takeoff
SET_FLIGHTMODE(quadcopterNumber, TAKEOFF_MODE);
cout << "Crazyflie " << +quadcopterNumber << " Taking off" << endl;
break;
case CMD_START_MOTORS:
SET_FLIGHTMODE(quadcopterNumber, IDLE_MODE);
crazyflie_info[quadcopterNumber-1].controllerData.resetSent = false;
break;
case CMD_GOTO_DEFAULT:
SET_X(quadcopterNumber, defaultSetpoints[quadcopterNumber-1][0]);
SET_Y(quadcopterNumber, defaultSetpoints[quadcopterNumber-1][1]);
SET_Z(quadcopterNumber, defaultSetpoints[quadcopterNumber-1][2]);
cout << "Crazyflie " << +quadcopterNumber << " Going to default coordinates" << endl;
break;
case CMD_LAND:
SET_FLIGHTMODE(quadcopterNumber, LANDING_MODE);
cout << "Crazyflie " << +quadcopterNumber << " Landing" << endl;
break;
case CMD_SET_X:
payload = atof(&tempCmd[2]);
SET_X(quadcopterNumber, payload);
cout << "Crazyflie " << +quadcopterNumber << " X Setpoint = " << payload << endl;
break;
case CMD_SET_Y:
payload = atof(&tempCmd[2]);
SET_Y(quadcopterNumber, payload);
cout << "Crazyflie " << +quadcopterNumber << " Y Setpoint = " << payload << endl;
break;
case CMD_SET_Z:
payload = atof(&cmd[2]);
SET_Z(quadcopterNumber, payload);
cout << "Crazyflie " << +quadcopterNumber << " Z Setpoint = " << payload << endl;
break;
case CMD_SET_YAW:
payload = atof(&cmd[2]);
SET_YAW(quadcopterNumber, payload);
cout << "Crazyflie " << +quadcopterNumber << " Yaw Setpoint = " << payload << endl;
break;
case CMD_ALL_MOTORS:
payload = atof(&cmd[2]);
SET_X(quadcopterNumber, payload);
SET_Y(quadcopterNumber, payload);
SET_Z(quadcopterNumber, payload);
SET_YAW(quadcopterNumber, payload);
cout << "Crazyflie " << +quadcopterNumber << " Setting motors to = " << payload << endl;
break;
case CMD_BRIDGE_HOVER:
// Determine where on the bridge to land
payload = atof(&cmd[2]);
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:
// Land on the bridge by just changing the Z setpoint
crazyflie_info[quadcopterNumber-1].controllerData.zSetpoint += 0.7;
SET_FLIGHTMODE(quadcopterNumber, BRIDGE_LANDING_MODE);
cout << "Crazyflie " << +quadcopterNumber << " Land on bridge" << endl;
break;
case CMD_BRIDGE_TAKEOFF:
SET_FLIGHTMODE(quadcopterNumber, BRIDGE_TAKEOFF_MODE);
cout << "Crazyflie " << +quadcopterNumber << " Take off from bridge" << endl;
break;
}
// Clear the command buffer
tempCmd[0] = 0;
}
......@@ -91,8 +91,6 @@ void* vrpn_go(void *threadID) {
printf("Spawning VRPN Thread\n");
while(!i) { //1 replace 'i<20000' when done testing ******
// Parse commands received from the user
parseCommand();
// This must be done before all other VRPN processing
connection->mainloop();
......
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