void ATCommandHandler::run() {
std::cout << std::endl << "AtCommand handler started and working" << std::endl;
navDataDemoMode();
fTrim();
takeOff();
watchdog();
move();
land();
}
int main ()
{
// ----- Required Code -------------------------
//Create new control thread
pthread_t ControlThread;
if(pthread_create(&ControlThread, NULL, DroneControl, NULL)) {
fprintf(stderr, "Error creating thread\n");
return 1;
}
// ---------------------------------------------
// The Following is an example of a basic flight
// Currently speeds have to be +/- 0.05,0.1,0.2 or 0.5
takeOff(); //take off
sleep(5); //Allows time for takeoff and settle before commands come in
rotateRight(0.5); //Rotate Right at 50% speed
sleep(2); //For 2 Seconds
hover(); //Hover in place
sleep(3); //For 3 Seconds
land(); //Land
// End Basic Flight Plan
// ---- Recommended Code ------------------------
terminateThread(); //Terminate Control Thread (should be called ONLY
//after landing for the last time)
// Stops Program terminating early by waiting for control thread
// to terminate properly.
if(pthread_join(ControlThread, NULL)) {
return 1;
}
//---------------------------------------------
return 0;
}
void AutoThread::run(){
QTime rate;
qDebug() << "automatic from: " << QThread::currentThreadId();
MavState previous = g::state;
MavState previous_platform = g::platform;
MavState next, next_platform;
land_count = 0;
rot_count = 0;
double vz = 0;
double vx = 0;
double vy = 0;
double e_x = 0;
double e_y = 0;
double e_z = 0;
float vy_platform = 0;
rate.start();
while (true) {
next = g::state;
next_platform = g::platform;
e_x = g::setPoint.x() - g::state.x();
e_y = g::setPoint.y() - g::state.y();
e_z = g::setPoint.z() - g::state.z();
vz = r_auto * (next.z() - previous.z()) ;
vx = r_auto * (next.x() - previous.x()) ;
vy = r_auto * (next.y() - previous.y()) ;
vy_platform = r_auto * (next_platform.y() - previous_platform.y()) ;
//takeoff
if(executioner::take_off::take_off_sig){
takeOff();
}
//land
if(executioner::land::land_sig){
position state;
state.x = g::state.x();
state.y = g::state.y();
state.z = g::state.z();
position p;
p.x = nodeList[actualNode].p.x;
p.y = nodeList[actualNode].p.y;
float vel = nodeList[actualNode].a.params[0];
land(vel,(float)1/r_auto,vz,p,state);
}
//move
if(executioner::move::move_sig){
position target;
target.x = nodeList[actualNode].p.x;
target.y = nodeList[actualNode].p.y;
target.z = nodeList[actualNode].p.z;
target.yaw = nodeList[actualNode].p.yaw;
float alpha = nodeList[actualNode].a.params[0];
position state;
state.x = g::state.x();
state.y = g::state.y();
state.z = g::state.z();
move(alpha,target,state);
}
//Land on moving platform (experimental)
if(executioner::land::land_plat_sig){
land_plat(g::platform,g::state,2);
}
//rotate
if(executioner::rotate::rotate_sig){
rotate();
}
//Trajectory
if(executioner::trajectory::traj_sig){
if(!executioner::trajectory::was_executing){
t_traj = 0;
executioner::trajectory::was_executing = true;
}
else{
t_traj += (float)1/r_auto;
double omega = nodeList[actualNode].a.params[0];
double radius = nodeList[actualNode].a.params[1];
double secs = nodeList[actualNode].a.params[2];
double look = nodeList[actualNode].a.params[3];
double c[2] = {nodeList[actualNode].p.x,nodeList[actualNode].p.y};
trajectory(omega,radius,c,t_traj,secs,look);
}
}
//Writing output file
float roll,pitch,yaw;
g::state.orientation2(&roll,&pitch,&yaw);
output <<g::state.x()<<" "<<g::state.y()<<" "<<g::state.z()<< //Position
" "<<g::setPoint.x()<<" "<<g::setPoint.y()<<" "<<g::setPoint.z()<< //Position SetPoint
" "<<e_x<<" "<<e_y<<" "<<e_z<<" "<< //Position error
vx <<" "<<vy<<" "<<vz<<" "<< //Velocity
roll<<" "<<pitch << " " << yaw //Attitude
<<" "<<plat_error[0]<<" "<<plat_error[1]; //platform allignement error
output << ";\n";
previous = next;
previous_platform = next_platform;
msleep(1000/r_auto - (float)rate.elapsed());
rate.restart();
}
}
void FlightController::run() {
double yUpperLimit = 9300;
double lowerLimit = 1500;
double xUpperLimit = 8100;
Route myRoute;
myRoute.initRoute(true);
ros::Rate loop_rate(LOOP_RATE);
setStraightFlight(true);
bool firstIteration = true;
takeOff();
bool turning = true;
double amountTurned = 0;
double turnStepSize = 30;
hoverDuration(3);
navData->resetRawRotation();
hoverDuration(2);
cmd.linear.z = 0.5;
pub_control.publish(cmd);
while (navData->getPosition().z < 1200)
ros::Rate(LOOP_RATE).sleep();
hoverDuration(1);
Vector3 pos;
double starting_orientation = navData->getRawRotation();
//ROS_INFO("Start while");
while (!dronePossision.positionLocked) {
//ROS_INFO("in while");
if (turning) {
turnDegrees(turnStepSize);
double orientation = navData->getRawRotation();
amountTurned += angleDifference(starting_orientation,orientation);
//ROS_INFO("AMOUNTTURNED: %f",amountTurned);
starting_orientation = orientation;
if (amountTurned >= 360)
turning = false;
} else {
navData->resetRaw();
flyForward(0.4);
turning = true;
amountTurned = 0;
}
}
lookingForQR = false;
cmd.linear.z = -0.5;
pub_control.publish(cmd);
while (navData->getPosition().z > 900)
ros::Rate(LOOP_RATE).sleep();
//ROS_INFO("end while");
navData->resetToPosition(dronePossision.x * 10, dronePossision.y * 10, dronePossision.heading);
int startWall = dronePossision.wallNumber;
turnDegrees(-dronePossision.angle);
if(startWall == 0) {
while (navData->getPosition().y - 700 > lowerLimit) {
flyBackward(0.7);
navData->addToY(-700);
hoverDuration(3);
}
strafe(1, 0.8);
navData->addToX(1000);
hoverDuration(2);
while (navData->getPosition().y + 700 < yUpperLimit) {
flyForward(0.5);
navData->addToX(700);
hoverDuration(3);
}
} else if(startWall == 2){
while (navData->getPosition().y + 700 < yUpperLimit) {
flyForward(0.5);
navData->addToX(700);
hoverDuration(3);
}
strafe(1, 0.8);
navData->addToX(-1000);
hoverDuration(2);
while (navData->getPosition().y - 700 > lowerLimit) {
flyBackward(0.7);
navData->addToY(-700);
hoverDuration(3);
}
} else if(startWall == 3) {
while (navData->getPosition().x + 700 < xUpperLimit){
flyBackward(0.7);
navData->addToX(700);
hoverDuration(3);
}
strafe(1, 0.8);
navData->addToY(1000);
hoverDuration(2);
while (navData->getPosition().x - 700 > xUpperLimit){
flyBackward(0.7);
navData->addToX(-700);
hoverDuration(3);
}
} else if(startWall == 1){
while (navData->getPosition().x + 700 < xUpperLimit){
flyBackward(0.7);
navData->addToX(700);
hoverDuration(3);
}
strafe(1, 0.8);
navData->addToY(-1000);
hoverDuration(2);
while (navData->getPosition().x - 700 > xUpperLimit){
flyBackward(0.7);
navData->addToX(-700);
hoverDuration(3);
}
}
/* else if(dronePossision.wallNumber == 1){
while (navData->getPosition().y - 1000 > 1500) {
flyForward(0.7);
navData->addToY(-1000);
hoverDuration(3);
cvHandler->swapCam(false);
cvHandler->checkCubes();
cvHandler->swapCam(true);
}
}*/
/*double rotation = navData->getRotation();
double target;
int wallNumber = dronePossision.wallNumber;
ROS_INFO("WALL NUMBER RECEIVED START: %d", wallNumber);
switch(dronePossision.wallNumber) {
case 0:
target = 180;
break;
case 1:
target = 270;
break;
case 2:
target = 0;
break;
case 3:
target = 90;
break;
}
ROS_INFO("Target = %f", target);*/
/*int i = 0;
while(i < 4) {
double difference = angleDifference(rotation, target);
int direction = angleDirection(rotation, target);
ROS_INFO("Difference = %f", difference);
ROS_INFO("Direction = %f", direction);
//turnDegrees(difference*direction);
cmd.angular.z = 1;
pub_control.publish(cmd);
while(navData->getRotation() > 190 || navData->getRotation() < 170 )
ros::Rate(50).sleep();
//ROS_INFO("Difference = %f", difference);
//ROS_INFO("Direction = %f", direction);
//turnTowardsAngle(target, 0);
/* lookingForQR = true;
flyForward(0.7);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
flyForward(0.7);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
flyForward(0.7);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
flyForward(0.7);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
lookingForQR = true;
strafe(0.7, -1);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
strafe(0.7, -1);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
strafe(0.7, -1);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
strafe(0.7, -1);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
if(target == 0)
while(navData->getPosition().y +1000 < 9300) {
flyForward(0.7);
navData->addToY(1000);
hoverDuration(4);
cvHandler->swapCam(false);
cvHandler->checkCubes();
cvHandler->swapCam(true);
if (navData->getRotation() != target) {
difference = angleDifference(rotation, target);
direction = angleDirection(rotation, target);
ROS_INFO("Difference = %f", difference);
ROS_INFO("Direction = %f", direction);
turnDegrees(difference * direction);
}
}
else if (target == 180)
while (navData->getPosition().y - 1000 > 1500) {
flyForward(0.7);
navData->addToY(-1000);
hoverDuration(4);
cvHandler->swapCam(false);
cvHandler->checkCubes();
cvHandler->swapCam(true);
if (navData->getRotation() != target) {
difference = angleDifference(rotation, target);
direction = angleDirection(rotation, target);
ROS_INFO("Difference = %f", difference);
ROS_INFO("Direction = %f", direction);
turnDegrees(difference * direction);
}
}
}
*/
land();
return;
}
