// --------------------------------------------------------------------------
// 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);
}
// --------------------------------------------------------------------------
// 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;
}
// --------------------------------------------------------------------------
// 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;
}
// --------------------------------------------------------------------------
// 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;
}
