Ejemplo n.º 1
0
// --------------------------------------------------------------------------
// controls()
// This function displays sensory data from the drone to the console.
// --------------------------------------------------------------------------
void display()
{
		clearScreen();
        // Orientation
        double roll  = ardrone.getRoll();
        double pitch = ardrone.getPitch();
        double yaw   = ardrone.getYaw();
        printf("Roll \t\t = %3.2f [deg]\n", roll  * RAD_TO_DEG);
        printf("Pitch\t\t = %3.2f [deg]\n", pitch * RAD_TO_DEG);
        printf("Yaw  \t\t = %3.2f [deg]\n", yaw   * RAD_TO_DEG);

        // Altitude
        double altitude = ardrone.getAltitude();
        printf("Altitude\t = %3.2f [m]\n", altitude);

        // Velocity
        double vx, vy, vz;
        double velocity = ardrone.getVelocity(&vx, &vy, &vz);
        printf("X Velocity\t = %3.2f [m/s]\n", vx);
        printf("Y Velocity\t = %3.2f [m/s]\n", vy);
        printf("Z Velocity\t = %3.2f [m/s]\n", vz);

        // Battery
        int battery = ardrone.getBatteryPercentage();
        printf("Battery\t\t = %d [%%]\n", battery);
}
Ejemplo n.º 2
0
// --------------------------------------------------------------------------
// main(Number of arguments, Argument values)
// Description  : This is the entry point of the program.
// Return value : SUCCESS:0  ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char *argv[])
{
    // AR.Drone class
    ARDrone ardrone;

    // Initialize
    if (!ardrone.open()) {
        std::cout << "Failed to initialize." << std::endl;
        return -1;
    }

    // Main loop
    while (1) {
        // Key input
        int key = cv::waitKey(33);
        if (key == 0x1b) break;

        // Get an image
        cv::Mat image= ardrone.getImage();

        // Orientation
        double roll  = ardrone.getRoll();
        double pitch = ardrone.getPitch();
        double yaw   = ardrone.getYaw();
        std::cout << "ardrone.roll  = " << roll  * RAD_TO_DEG << " [deg]" << std::endl;
        std::cout << "ardrone.pitch = " << pitch * RAD_TO_DEG << " [deg]" << std::endl;
        std::cout << "ardrone.yaw   = " << yaw   * RAD_TO_DEG << " [deg]" << std::endl;

        // Altitude
        double altitude = ardrone.getAltitude();
        std::cout << "ardrone.altitude = " << altitude << " [m]" << std::endl;

        // Velocity
        double vx, vy, vz;
        double velocity = ardrone.getVelocity(&vx, &vy, &vz);
        std::cout << "ardrone.vx = " << vx << " [m/s]" << std::endl;
        std::cout << "ardrone.vy = " << vy << " [m/s]" << std::endl;
        std::cout << "ardrone.vz = " << vz << " [m/s]" << std::endl;

        // Battery
        int battery = ardrone.getBatteryPercentage();
        std::cout << "ardrone.battery = " << battery << " [%%]" << std::endl;

        // Take off / Landing 
        if (key == ' ') {
            if (ardrone.onGround()) ardrone.takeoff();
            else                    ardrone.landing();
        }

        // Move
        double x = 0.0, y = 0.0, z = 0.0, r = 0.0;
        if (key == 0x260000) x =  1.0;
        if (key == 0x280000) x = -1.0;
        if (key == 0x250000) r =  1.0;
        if (key == 0x270000) r = -1.0;
        ardrone.move3D(x, y, z, r);

        // Change camera
        static int mode = 0;
        if (key == 'c') ardrone.setCamera(++mode%4);

        // Display the image
        cv::imshow("camera", image);
    }

    // See you
    ardrone.close();

    return 0;
}
Ejemplo n.º 3
0
// --------------------------------------------------------------------------
// main(Number of arguments, Argument values)
// Description  : This is the entry point of the program.
// Return value : SUCCESS:0  ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
    // AR.Drone class
    ARDrone ardrone;

    // Initialize
    if (!ardrone.open()) {
        printf("Failed to initialize.\n");
        return -1;
    }

    // Main loop
    while (1) {
        // Key input
        int key = cvWaitKey(33);
        if (key == 0x1b) break;

        // Update
        if (!ardrone.update()) break;

        // Get an image
        IplImage *image = ardrone.getImage();

        // Orientation
        double roll  = ardrone.getRoll();
        double pitch = ardrone.getPitch();
        double yaw   = ardrone.getYaw();
        printf("ardrone.roll  = %3.2f [deg]\n", roll  * RAD_TO_DEG);
        printf("ardrone.pitch = %3.2f [deg]\n", pitch * RAD_TO_DEG);
        printf("ardrone.yaw   = %3.2f [deg]\n", yaw   * RAD_TO_DEG);

        // Altitude
        double altitude = ardrone.getAltitude();
        printf("ardrone.altitude = %3.2f [m]\n", altitude);

        // Velocity
        double vx, vy, vz;
        double velocity = ardrone.getVelocity(&vx, &vy, &vz);
        printf("ardrone.vx = %3.2f [m/s]\n", vx);
        printf("ardrone.vy = %3.2f [m/s]\n", vy);
        printf("ardrone.vz = %3.2f [m/s]\n", vz);

        // Battery
        int battery = ardrone.getBatteryPercentage();
        printf("ardrone.battery = %d [%%]\n", battery);

        // Take off / Landing 
        if (key == ' ') {
            if (ardrone.onGround()) ardrone.takeoff();
            else                    ardrone.landing();
        }

        // Move
        double x = 0.0, y = 0.0, z = 0.0, r = 0.0;
        if (key == 0x260000) x =  1.0;
        if (key == 0x280000) x = -1.0;
        if (key == 0x250000) r =  1.0;
        if (key == 0x270000) r = -1.0;
        ardrone.move3D(x, y, z, r);

        // Change camera
        static int mode = 0;
        if (key == 'c') ardrone.setCamera(++mode%4);

        // Display the image
        cvShowImage("camera", image);
    }

    // See you
    ardrone.close();

    return 0;
}
// --------------------------------------------------------------------------
// main(Number of arguments, Argument values)
// Description  : This is the entry point of the program.
// Return value : SUCCESS:0  ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
    // AR.Drone class
    ARDrone ardrone;

    // Initialize
    if (!ardrone.open()) {
        printf("Failed to initialize.\n");
        return -1;
    }

    // Battery
    printf("Battery = %d%%\n", ardrone.getBatteryPercentage());

    // Map
    cv::Mat map = cv::Mat::zeros(500, 500, CV_8UC3);

    // Kalman filter
    cv::KalmanFilter kalman(6, 4, 0);

    // Sampling time [s]
    const double dt = 0.033;

    // Transition matrix (x, y, z, vx, vy, vz)
    cv::Mat1f F(6, 6);
    F << 1.0, 0.0, 0.0,  dt, 0.0, 0.0,
         0.0, 1.0, 0.0, 0.0,  dt, 0.0,
         0.0, 0.0, 1.0, 0.0, 0.0,  dt,
         0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
         0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
         0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
    kalman.transitionMatrix = F;

    // Measurement matrix (0, 0, z, vx, vy, vz)
    cv::Mat1f H(4, 6);
    H << 0, 0, 1, 0, 0, 0,
         0, 0, 0, 1, 0, 0,
         0, 0, 0, 0, 1, 0,
         0, 0, 0, 0, 0, 1;
    kalman.measurementMatrix = H;

    // Process noise covairance (x, y, z, vx, vy, vz)
    cv::Mat1f Q(6, 6);
    Q << 0.1, 0.0, 0.0, 0.0, 0.0, 0.0,
         0.0, 0.1, 0.0, 0.0, 0.0, 0.0,
         0.0, 0.0, 0.1, 0.0, 0.0, 0.0,
         0.0, 0.0, 0.0, 0.3, 0.0, 0.0,
         0.0, 0.0, 0.0, 0.0, 0.3, 0.0,
         0.0, 0.0, 0.0, 0.0, 0.0, 0.3;
    kalman.processNoiseCov = Q;

    // Measurement noise covariance (z, vx, vy, vz)
    cv::Mat1f R(4, 4);
    R << 0.1, 0.0, 0.00, 0.00,
         0.0, 0.1, 0.00, 0.00,
         0.0, 0.0, 0.05, 0.00,
         0.0, 0.0, 0.00, 0.05;
    kalman.measurementNoiseCov = R;

    // Main loop
    while (1) {
        // Key input
        int key = cv::waitKey(33);
        if (key == 0x1b) break;

        // Update
        if (!ardrone.update()) break;

        // Get an image
        cv::Mat image = ardrone.getImage();

        // Prediction
        cv::Mat prediction = kalman.predict();

        // Altitude
        double altitude = ardrone.getAltitude();

        // Orientations
        double roll  = ardrone.getRoll();
        double pitch = ardrone.getPitch();
        double yaw   = ardrone.getYaw();

        // Velocities
        double vx, vy, vz;
        double velocity = ardrone.getVelocity(&vx, &vy, &vz);
        cv::Mat V  = (cv::Mat1f(3,1) << vx, vy, vz);

        // Rotation matrices
        cv::Mat RZ = (cv::Mat1f(3,3) <<   cos(yaw), -sin(yaw),        0.0,
                                          sin(yaw),  cos(yaw),        0.0,
                                               0.0,       0.0,        1.0);
        cv::Mat RY = (cv::Mat1f(3,3) << cos(pitch),       0.0,  sin(pitch),
                                               0.0,       1.0,        0.0,
                                       -sin(pitch),       0.0,  cos(pitch));
        cv::Mat RX = (cv::Mat1f(3,3) <<        1.0,       0.0,        0.0,
                                               0.0, cos(roll), -sin(roll),
                                               0.0, sin(roll),  cos(roll));

        // Time [s]
        static int64 last = cv::getTickCount();
        double dt = (cv::getTickCount() - last) / cv::getTickFrequency();
        last = cv::getTickCount();

        // Local movements (z, vx, vy, vz)
        cv::Mat1f M = RZ * RY * RX * V * dt;
        cv::Mat measurement = (cv::Mat1f(4,1) << altitude, M(0,0), M(1,0), M(2,0));

        // Correction
        cv::Mat1f estimated = kalman.correct(measurement);

        // Position (x, y, z)
        double pos[3] = {estimated(0,0), estimated(1,0), estimated(2,0)};
        printf("x = %3.2fm, y = %3.2fm, z = %3.2fm\n", pos[0], pos[1], pos[2]);

        // Take off / Landing 
        if (key == ' ') {
            if (ardrone.onGround()) ardrone.takeoff();
            else                    ardrone.landing();
        }

        // Move
        double x = 0.0, y = 0.0, z = 0.0, r = 0.0;
        if (key == 0x260000) x =  1.0;
        if (key == 0x280000) x = -1.0;
        if (key == 0x250000) r =  1.0;
        if (key == 0x270000) r = -1.0;
        if (key == 'q')      z =  1.0;
        if (key == 'a')      z = -1.0;
        ardrone.move3D(x, y, z, r);

        // Change camera
        static int mode = 0;
        if (key == 'c') ardrone.setCamera(++mode%4);

        // Display the image
        cv::circle(map, cv::Point(-pos[1]*100.0 + map.cols/2, -pos[0]*100.0 + map.rows/2), 2, CV_RGB(255,0,0));
        cv::imshow("map", map);
        cv::imshow("camera", image);
    }

    // See you
    ardrone.close();

    return 0;
}
Ejemplo n.º 5
0
// --------------------------------------------------------------------------
// main(Number of arguments, Argument values)
// Description  : This is the entry point of the program.
// Return value : SUCCESS:0  ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char *argv[])
{
    // AR.Drone class
    ARDrone ardrone;

    // Initialize
    if (!ardrone.open()) {
        std::cout << "Failed to initialize." << std::endl;
        return -1;
    }

    // Battery
    std::cout << "Battery = " << ardrone.getBatteryPercentage() << " [%]" << std::endl;

    // Map
    cv::Mat map = cv::Mat::zeros(500, 500, CV_8UC3);

    // Position matrix
    cv::Mat P = cv::Mat::zeros(3, 1, CV_64FC1);

    // Main loop
    while (1) {
        // Key input
        int key = cv::waitKey(33);
        if (key == 0x1b) break;

        // Get an image
        cv::Mat image = ardrone.getImage();

        // Altitude
        double altitude = ardrone.getAltitude();

        // Orientations
        double roll = ardrone.getRoll();
        double pitch = ardrone.getPitch();
        double yaw = ardrone.getYaw();

        // Velocities
        double vx, vy, vz;
        double velocity = ardrone.getVelocity(&vx, &vy, &vz);
        cv::Mat V = (cv::Mat1f(3, 1) << vx, vy, vz);

        // Rotation matrices
        cv::Mat RZ = (cv::Mat1f(3, 3) << cos(yaw), -sin(yaw), 0.0,
                                         sin(yaw),  cos(yaw), 0.0,
                                              0.0,       0.0, 1.0);
        cv::Mat RY = (cv::Mat1f(3, 3) << cos(pitch), 0.0, sin(pitch),
                                                0.0, 1.0,        0.0,
                                        -sin(pitch), 0.0, cos(pitch));
        cv::Mat RX = (cv::Mat1f(3, 3) << 1.0,       0.0,        0.0,
                                         0.0, cos(roll), -sin(roll),
                                         0.0, sin(roll),  cos(roll));

        // Time [s]
        static int64 last = cv::getTickCount();
        double dt = (cv::getTickCount() - last) / cv::getTickFrequency();
        last = cv::getTickCount();

        // Dead-reckoning
        P = P + RZ * RY * RX * V * dt;

        // Position (x, y, z)
        double pos[3] = { P.at<double>(0, 0), P.at<double>(1, 0), P.at<double>(2, 0) };
        std::cout << "x = " << pos[0] << "[m], " << "y = " << pos[1] << "[m], " << "z = " << pos[2] << "[m]" << std::endl;

        // Take off / Landing 
        if (key == ' ') {
            if (ardrone.onGround()) ardrone.takeoff();
            else                    ardrone.landing();
        }

        // Move
        double x = 0.0, y = 0.0, z = 0.0, r = 0.0;
        if (key == 'i' || key == CV_VK_UP)    vx =  1.0;
        if (key == 'k' || key == CV_VK_DOWN)  vx = -1.0;
        if (key == 'u' || key == CV_VK_LEFT)  vr =  1.0;
        if (key == 'o' || key == CV_VK_RIGHT) vr = -1.0;
        if (key == 'j') vy =  1.0;
        if (key == 'l') vy = -1.0;
        if (key == 'q') vz =  1.0;
        if (key == 'a') vz = -1.0;
        ardrone.move3D(x, y, z, r);

        // Change camera
        static int mode = 0;
        if (key == 'c') ardrone.setCamera(++mode % 4);

        // Display the image
        cv::circle(map, cv::Point(-pos[1] * 100.0 + map.cols / 2, -pos[0] * 100.0 + map.rows / 2), 2, CV_RGB(255, 0, 0));
        cv::imshow("map", map);
        cv::imshow("camera", image);
    }

    // See you
    ardrone.close();

    return 0;
}