<commit_before>#include "FlightController.h"

void *startNavdata(void *thread_args);

void *startCV(void *thread_args);

void *startController(void *thread_arg);

void *runQR(void *thread_arg);

struct thread_data {
    Nav *navData;
    CV_Handler *cvHandler;
    FlightController *controller;
    ros::NodeHandle *n;
} myThreadData;

FlightController::FlightController() {

    cmd.linear.x = 0.0;
    cmd.linear.y = 0.0;
    cmd.linear.z = 0.0;

    cmd.angular.x = 0.0;
    cmd.angular.y = 0.0;
    cmd.angular.z = 0.0;

    straightFlight = false;

    LOOP_RATE = 50;


    ros::NodeHandle nh;

    if (nh.hasParam("TAKEOFF_TIME"))
        nh.getParam("TAKEOFF_TIME", TAKEOFF_TIME);
    else
        TAKEOFF_TIME = 6;

    if (nh.hasParam("TOLERANCE"))
        nh.getParam("TOLERANCE", TOLERANCE);
    else
        TOLERANCE = 100;

    if (nh.hasParam("TRANSIT_SPEED"))
        nh.getParam("TRANSIT_SPEED", TRANSIT_SPEED);
    else
        TRANSIT_SPEED = 0.05;

    if (nh.hasParam("CRUISE_SPEED"))
        nh.getParam("CRUISE_SPEED", CRUISE_SPEED);
    else
        CRUISE_SPEED = 0.01;

    if (nh.hasParam("CRUISE_LIMIT"))
        nh.getParam("CRUISE_LIMIT", CRUISE_LIMIT);
    else
        CRUISE_LIMIT = 300;

    if (nh.hasParam("CONTROL_SLEEP"))
        nh.getParam("CONTROL_SLEEP", CONTROL_SLEEP);
    else
        CONTROL_SLEEP = 10;

    control_loop = ros::Rate(CONTROL_SLEEP);

}

FlightController::FlightController(int loopRate, ros::NodeHandle *nh, Nav *nav) : FlightController() {
    LOOP_RATE = loopRate;

    pub_land = nh->advertise<std_msgs::Empty>("/ardrone/land", 1);
    pub_takeoff = nh->advertise<std_msgs::Empty>("/ardrone/takeoff", 1);
    pub_control = nh->advertise<geometry_msgs::Twist>("cmd_vel", 1);
    pub_reset = nh->advertise<std_msgs::Empty>("/ardrone/reset", 1);

    cvHandler = new CV_Handler();
    qr = new QR(cvHandler);

    navData = nav;

    myThreadData.cvHandler = cvHandler;
    myThreadData.navData = navData;
    myThreadData.n = nh;

    pthread_t threads[2];
    pthread_create(&threads[0], NULL, startCV, &myThreadData);
    pthread_create(&threads[1], NULL, startNavdata, &myThreadData);


}

// Destructor
FlightController::~FlightController() {
    delete (cvHandler);
    delete (qr);
}

void FlightController::run() {
    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 < 1000)
        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;

    ROS_INFO("end while");
    navData->resetToPosition(dronePossision.x * 10, dronePossision.y * 10, dronePossision.heading);

    double rotation = navData->getRotation();
    double target;
    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);

    double difference = angleDifference(rotation, target);<commit_msg>made the drone drop again<commit_after>#include "FlightController.h"

void *startNavdata(void *thread_args);

void *startCV(void *thread_args);

void *startController(void *thread_arg);

void *runQR(void *thread_arg);

struct thread_data {
    Nav *navData;
    CV_Handler *cvHandler;
    FlightController *controller;
    ros::NodeHandle *n;
} myThreadData;

FlightController::FlightController() {

    cmd.linear.x = 0.0;
    cmd.linear.y = 0.0;
    cmd.linear.z = 0.0;

    cmd.angular.x = 0.0;
    cmd.angular.y = 0.0;
    cmd.angular.z = 0.0;

    straightFlight = false;

    LOOP_RATE = 50;


    ros::NodeHandle nh;

    if (nh.hasParam("TAKEOFF_TIME"))
        nh.getParam("TAKEOFF_TIME", TAKEOFF_TIME);
    else
        TAKEOFF_TIME = 6;

    if (nh.hasParam("TOLERANCE"))
        nh.getParam("TOLERANCE", TOLERANCE);
    else
        TOLERANCE = 100;

    if (nh.hasParam("TRANSIT_SPEED"))
        nh.getParam("TRANSIT_SPEED", TRANSIT_SPEED);
    else
        TRANSIT_SPEED = 0.05;

    if (nh.hasParam("CRUISE_SPEED"))
        nh.getParam("CRUISE_SPEED", CRUISE_SPEED);
    else
        CRUISE_SPEED = 0.01;

    if (nh.hasParam("CRUISE_LIMIT"))
        nh.getParam("CRUISE_LIMIT", CRUISE_LIMIT);
    else
        CRUISE_LIMIT = 300;

    if (nh.hasParam("CONTROL_SLEEP"))
        nh.getParam("CONTROL_SLEEP", CONTROL_SLEEP);
    else
        CONTROL_SLEEP = 10;

    control_loop = ros::Rate(CONTROL_SLEEP);

}

FlightController::FlightController(int loopRate, ros::NodeHandle *nh, Nav *nav) : FlightController() {
    LOOP_RATE = loopRate;

    pub_land = nh->advertise<std_msgs::Empty>("/ardrone/land", 1);
    pub_takeoff = nh->advertise<std_msgs::Empty>("/ardrone/takeoff", 1);
    pub_control = nh->advertise<geometry_msgs::Twist>("cmd_vel", 1);
    pub_reset = nh->advertise<std_msgs::Empty>("/ardrone/reset", 1);

    cvHandler = new CV_Handler();
    qr = new QR(cvHandler);

    navData = nav;

    myThreadData.cvHandler = cvHandler;
    myThreadData.navData = navData;
    myThreadData.n = nh;

    pthread_t threads[2];
    pthread_create(&threads[0], NULL, startCV, &myThreadData);
    pthread_create(&threads[1], NULL, startNavdata, &myThreadData);


}

// Destructor
FlightController::~FlightController() {
    delete (cvHandler);
    delete (qr);
}

void FlightController::run() {
    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;

    ROS_INFO("end while");
    navData->resetToPosition(dronePossision.x * 10, dronePossision.y * 10, dronePossision.heading);

    cmd.linear.z = -0.5;
    pub_control.publish(cmd);
    while (navData->getPosition().z < 1000)
        ros::Rate(LOOP_RATE).sleep();


    double rotation = navData->getRotation();
    double target;
    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);

    double difference = angleDifference(rotation, target);
{'fraction_non_alphanumeric': 0.12758827049670857, 'fraction_numerical': 0.017953321364452424, 'mean_word_length': 2.6649122807017545, 'pattern_counts': {'":': 0, '<': 14, '<?xml version=': 0, '>': 36, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3862823', 'n_tokens_mistral': 3037, 'n_tokens_neox': 2772, 'n_words': 506}