diff --git a/TelloController.pro.user b/TelloController.pro.user index d4ea072..27ce227 100644 --- a/TelloController.pro.user +++ b/TelloController.pro.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/include/facedetector.h b/include/facedetector.h index 535f9f0..5e0f9c7 100644 --- a/include/facedetector.h +++ b/include/facedetector.h @@ -2,6 +2,7 @@ #define FACEDETECTOR_H #include +#include #include #include @@ -22,6 +23,7 @@ class FaceDetector : public QObject signals: void faceframe(cv::Mat matrix); + void faceoffset(QVector3D& offset); public slots: void detect(cv::Mat src); diff --git a/include/mainwindow.h b/include/mainwindow.h index 90ada9e..30e3266 100644 --- a/include/mainwindow.h +++ b/include/mainwindow.h @@ -8,6 +8,9 @@ #include #include #include +#include +#include +#include #include "../include/tellocontroller.h" #include "../include/videoreader.h" @@ -72,9 +75,14 @@ private slots: void on_videoframe(cv::Mat matrix); void on_record_timer(); void on_alert_timer(); + void on_faceoffset(QVector3D& offset); void on_button_start_recording_clicked(); void on_splitter_splitterMoved(int pos, int index); + void on_button_add_waypoint_clicked(); + + void on_button_remove_waypoint_clicked(); + void on_waypoints_list_itemSelectionChanged(); protected: void keyPressEvent(QKeyEvent *event) override; @@ -90,11 +98,14 @@ private slots: QThread* face_detector_thread; EdgeDetector* edge_detector; FaceDetector* face_detector; - std::unique_ptr progress_bar; + std::unique_ptr battery_progress_bar; + std::unique_ptr wifi_progress_bar; + std::unique_ptr temperature_progress_bar; std::unique_ptr controller; std::unique_ptr poll_infos_timer; std::unique_ptr record_timer; std::unique_ptr alert_timer; + QVector waypoints; bool is_connected; bool is_video_started; bool is_flying; diff --git a/mainwindow.ui b/mainwindow.ui index 65324c2..83d527f 100644 --- a/mainwindow.ui +++ b/mainwindow.ui @@ -19,6 +19,9 @@ MainWindow + + + true @@ -406,71 +409,103 @@ Actions - - - - - - 0 - 0 - - - - Take off - - - - - - - - 0 - 0 - - - - Start video - - - - - - - - 0 - 0 - - - - Start recording - - - - - - - - 0 - 0 - - - - - 0 - 50 - - - - QPushButton { + + + + 10 + 27 + 161 + 24 + + + + + 0 + 0 + + + + Take off + + + + + + 10 + 50 + 161 + 24 + + + + + 0 + 0 + + + + Start video + + + + + + 10 + 73 + 161 + 24 + + + + + 0 + 0 + + + + Start recording + + + + + + 10 + 105 + 161 + 61 + + + + + 0 + 0 + + + + + 0 + 0 + + + + QPushButton { color: red; + + font-size: 16px; + background: transparent; + border: 1px solid red; +} + +QPushButton:hover:!pressed { + color: red; + background: #f0d4d4; + border: 1px solid red; } - - - EMERGENCY - - - - + + + EMERGENCY + + @@ -709,8 +744,8 @@ - 0 - 0 + 10 + 10 180 220 @@ -742,9 +777,9 @@ } QGroupBox{ - background: rgba(0,0,0,0.6); + background: rgba(0,0,0,0.8); color: white; - border: none; + border: 1px solid #333; } @@ -757,28 +792,25 @@ QGroupBox{ true - - + + 0 0 - + - 80 - 0 + 50 + 16777215 - - The maximum speed allowed by the drone - - - QLabel{color: white} - - Max. speed + 0 cm + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter @@ -804,46 +836,33 @@ QGroupBox{ - - - - - 80 - 0 - - - - The actual height in centimers of the drone - - - Height - - - - - + + 0 0 - + - 50 - 16777215 + 80 + 0 - - 0 cm + + The maximum speed allowed by the drone - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + QLabel{color: white} + + + Max. speed - - + + 80 @@ -851,10 +870,10 @@ QGroupBox{ - Inertial Measurement Unit: Yaw, Pitch, Roll + The atmospheric pressure in millibars - I.M.U + Pressure @@ -896,8 +915,8 @@ QGroupBox{ - - + + 0 @@ -906,20 +925,20 @@ QGroupBox{ - 80 + 50 16777215 - 0, 0, 0 + 0 s Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - + + 80 @@ -927,37 +946,15 @@ QGroupBox{ - Time Of Flight: Height in centimers relative from the take off point - - - T.O.F - - - - - - - - 0 - 0 - - - - - 50 - 16777215 - + The drone temperature in degrees celcius - 0 cm - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Temperature - - + + 80 @@ -965,15 +962,15 @@ QGroupBox{ - The flight time session in seconds + The actual height in centimers of the drone - Flight time + Height - - + + 0 @@ -987,29 +984,13 @@ QGroupBox{ - 0 s + 0 cm Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - 80 - 0 - - - - The drone temperature in degrees celcius - - - Temperature - - - @@ -1032,22 +1013,6 @@ QGroupBox{ - - - - - 80 - 0 - - - - The atmospheric pressure in millibars - - - Pressure - - - @@ -1070,14 +1035,24 @@ QGroupBox{ - - - - - 0 - 0 - + + + + + 80 + 0 + + + + Inertial Measurement Unit: Yaw, Pitch, Roll + + + I.M.U + + + + 80 @@ -1085,41 +1060,51 @@ QGroupBox{ - The Wifi Signal Noise Ratio + Time Of Flight: Height in centimers relative from the take off point - Wifi SNR + T.O.F - - + + 0 0 - - - 30 - 0 - - - 50 + 80 16777215 - 0 + 0, 0, 0 Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + 80 + 0 + + + + The flight time session in seconds + + + Flight time + + + @@ -1194,13 +1179,59 @@ QGroupBox{ Waypoints + + + + 10 + 10 + 80 + 24 + + + + Add + + + + + + 90 + 10 + 80 + 24 + + + + Remove + + + false + + + + + + 10 + 40 + 161 + 201 + + + + QAbstractItemView::MultiSelection + + - + + + true + + diff --git a/src/facedetector.cpp b/src/facedetector.cpp index bb911d5..21f03da 100644 --- a/src/facedetector.cpp +++ b/src/facedetector.cpp @@ -22,9 +22,30 @@ void FaceDetector::detect(cv::Mat src) std::vector faces = {}; classifier.detectMultiScale(src, faces, scale_factor, min_neighbors); + + int center_x = int(960/2); + int center_y = int(720/2); + cv::circle(src, cv::Size(center_x, center_y), 10, cv::Scalar(0, 255, 0)); + int face_center_x = center_x; + int face_center_y = center_y; + int z_area = 0; + for (size_t i = 0; i < faces.size(); i++) { cv::rectangle(src, faces[i].tl(), faces[i].br(), cv::Scalar(50, 50, 255), 3); + + face_center_x = faces[i].x + int(faces[i].width/2); + face_center_y = faces[i].y + int(faces[i].height/2); + z_area = faces[i].width * faces[i].height; + + cv::circle(src, cv::Size(face_center_x, face_center_y), 10, cv::Scalar(0, 255, 0)); } + cv::line(src, cv::Point(center_x, center_y), cv::Point(face_center_x, face_center_y), cv::Scalar(0, 255, 0), 1); + + int offset_x = face_center_x - center_x; + int offset_y = face_center_y - center_y - 30; + + auto vec = QVector3D(offset_x, offset_y, z_area); + emit faceoffset(vec); emit faceframe(src); } diff --git a/src/mainwindow.cpp b/src/mainwindow.cpp index 768c004..7f50488 100644 --- a/src/mainwindow.cpp +++ b/src/mainwindow.cpp @@ -4,7 +4,8 @@ MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent), ui(new Ui::MainWindow) - , progress_bar(nullptr), + , battery_progress_bar(nullptr), + wifi_progress_bar(nullptr), controller(nullptr), is_connected(false), is_video_started(false), @@ -24,6 +25,9 @@ MainWindow::MainWindow(QWidget *parent) ui->recording_dot->setVisible(false); ui->recording_text->setVisible(false); ui->alert_message->setVisible(false); + ui->button_remove_waypoint->setDisabled(true); + ui->statusbar->setStyleSheet("background-color: rgb(220, 220, 220);"); + enable_flight_controls(false); QPixmap pxr(960, 720); @@ -33,16 +37,38 @@ MainWindow::MainWindow(QWidget *parent) video_recorder = new VideoRecorder(); - auto progress_bar_ptr = new QProgressBar(this); - progress_bar_ptr->setDisabled(true); - progress_bar_ptr->setRange(0, 100); - progress_bar_ptr->setValue(0); - progress_bar_ptr->setAlignment(Qt::AlignVCenter); - progress_bar_ptr->setMaximumSize(180, 19); - progress_bar_ptr->setTextVisible(true); - progress_bar_ptr->setFormat("Battery (0 %)"); - ui->statusbar->addPermanentWidget(progress_bar_ptr, 2); - progress_bar.reset(progress_bar_ptr); + auto battery_progress_bar_ptr = new QProgressBar(this); + battery_progress_bar_ptr->setDisabled(true); + battery_progress_bar_ptr->setRange(0, 100); + battery_progress_bar_ptr->setValue(0); + battery_progress_bar_ptr->setAlignment(Qt::AlignVCenter); + battery_progress_bar_ptr->setMaximumSize(180, 19); + battery_progress_bar_ptr->setTextVisible(true); + battery_progress_bar_ptr->setFormat("Battery (0 %)"); + ui->statusbar->addPermanentWidget(battery_progress_bar_ptr, 2); + battery_progress_bar.reset(battery_progress_bar_ptr); + + auto wifi_progress_bar_ptr = new QProgressBar(this); + wifi_progress_bar_ptr->setDisabled(true); + wifi_progress_bar_ptr->setRange(0, 100); + wifi_progress_bar_ptr->setValue(0); + wifi_progress_bar_ptr->setAlignment(Qt::AlignVCenter); + wifi_progress_bar_ptr->setMaximumSize(180, 19); + wifi_progress_bar_ptr->setTextVisible(true); + wifi_progress_bar_ptr->setFormat("Wifi (0 %)"); + ui->statusbar->addPermanentWidget(wifi_progress_bar_ptr, 2); + wifi_progress_bar.reset(wifi_progress_bar_ptr); + + auto temperature_progress_bar_ptr = new QProgressBar(this); + temperature_progress_bar_ptr->setDisabled(true); + temperature_progress_bar_ptr->setRange(0, 100); + temperature_progress_bar_ptr->setValue(0); + temperature_progress_bar_ptr->setAlignment(Qt::AlignVCenter); + temperature_progress_bar_ptr->setMaximumSize(180, 19); + temperature_progress_bar_ptr->setTextVisible(true); + temperature_progress_bar_ptr->setFormat("Temperature (0°C)"); + ui->statusbar->addPermanentWidget(temperature_progress_bar_ptr, 2); + temperature_progress_bar.reset(temperature_progress_bar_ptr); } MainWindow::~MainWindow() @@ -61,9 +87,12 @@ void MainWindow::on_button_connect_clicked() ui->button_takeoff->setText("Take off"); ui->button_takeoff->setDisabled(true); ui->button_emergency->setDisabled(true); - progress_bar->setDisabled(true); - progress_bar->setValue(0); - progress_bar->setFormat("Battery (0 %)"); + battery_progress_bar->setDisabled(true); + battery_progress_bar->setValue(0); + battery_progress_bar->setFormat("Battery (0 %)"); + wifi_progress_bar->setDisabled(true); + wifi_progress_bar->setValue(0); + wifi_progress_bar->setFormat("Wifi (0 %)"); ui->statusbar->showMessage("Disconnected"); is_connected = false; is_video_started = false; @@ -96,7 +125,6 @@ void MainWindow::on_button_connect_clicked() void MainWindow::on_connected() { - progress_bar->setVisible(true); ui->input_ip->setDisabled(true); ui->input_port->setDisabled(true); ui->button_connect->setText("DISCONNECT"); @@ -104,29 +132,32 @@ void MainWindow::on_connected() ui->button_takeoff->setDisabled(false); ui->button_emergency->setDisabled(false); ui->button_start_video->setDisabled(false); - progress_bar->setDisabled(false); + battery_progress_bar->setVisible(true); + battery_progress_bar->setDisabled(false); + wifi_progress_bar->setVisible(true); + wifi_progress_bar->setDisabled(false); is_connected = true; on_pollinfos(); auto ptr = new QTimer(this); connect(ptr, SIGNAL(timeout()), this, SLOT(on_pollinfos())); poll_infos_timer.reset(ptr); -// poll_infos_timer->start(3000); + poll_infos_timer->start(2500); } void MainWindow::on_battery(int percent) { - if (percent < 10 && !is_alerting && is_flying) { + if (percent < 10 && !is_alerting) { auto ptr = new QTimer(this); connect(ptr, SIGNAL(timeout()), this, SLOT(on_alert_timer())); alert_timer.reset(ptr); alert_timer->start(1000); is_alerting = true; - ui->alert_message->setText("BATTERY ALERT\n" + QString::number(percent) + " %\nLAND QUICKLY !"); + ui->alert_message->setText("BATTERY ALERT\n" + QString::number(percent) + " %\nFIND A PLACE TO LAND"); } - progress_bar->setValue(percent); - progress_bar->setFormat("Battery (" + QString::number(percent) + " %)"); + battery_progress_bar->setValue(percent); + battery_progress_bar->setFormat("Battery (" + QString::number(percent) + " %)"); } void MainWindow::on_maxspeed(float value) { @@ -142,16 +173,18 @@ void MainWindow::on_height(int centimeters) { } void MainWindow::on_temperature(int degrees) { - if (degrees > 90 && !is_alerting && is_flying) { + if (degrees > 90 && !is_alerting) { auto ptr = new QTimer(this); connect(ptr, SIGNAL(timeout()), this, SLOT(on_alert_timer())); alert_timer.reset(ptr); alert_timer->start(1000); is_alerting = true; - ui->alert_message->setText("TEMPERATURE ALERT\n" + QString::number(degrees) + "°C\nLAND QUICKLY !"); + ui->alert_message->setText("TEMPERATURE ALERT\n" + QString::number(degrees) + "°C\nFIND A PLACE TO LAND"); } ui->drone_temperature->setText(QString::number(degrees) + "°C"); + temperature_progress_bar->setValue(degrees); + temperature_progress_bar->setFormat("Temperature (" + QString::number(degrees) + "°C)"); } void MainWindow::on_imu(const QVector3D &v) { @@ -171,23 +204,25 @@ void MainWindow::on_timeofflight(float meters) { } void MainWindow::on_wifisnr(int ratio) { - ui->drone_wifi->setText(QString::number(ratio)); + wifi_progress_bar->setValue(ratio); + wifi_progress_bar->setFormat("Wifi (" + QString::number(ratio) + " %)"); } void MainWindow::on_pollinfos() { if (is_connected) { const QList commands { - "speed?", - "battery?", - "time?", - "height?", "temp?", - "attitude?", - "baro?", - "acceleration?", - "tof?", + "battery?", "wifi?" +// "speed?", +// "time?", +// "height?", +// "attitude?", +// "baro?", +// "acceleration?", +// "tof?", + }; for (auto& command : commands) @@ -370,6 +405,7 @@ void MainWindow::on_button_start_video_clicked() face_detector_thread = new QThread(); face_detector = new FaceDetector(); + connect(face_detector, SIGNAL(faceoffset(QVector3D&)), this, SLOT(on_faceoffset(QVector3D&))); face_detector->moveToThread(face_detector_thread); face_detector_thread->start(); @@ -416,7 +452,7 @@ void MainWindow::on_button_move_left_clicked() void MainWindow::on_button_move_forward_clicked() { if (is_connected && is_flying) { - controller->add_command("forward 20"); + controller->add_command("forward 100"); controller->flush(); } } @@ -565,6 +601,47 @@ void MainWindow::on_alert_timer() ui->alert_message->setVisible(!ui->alert_message->isVisible()); } +// Move drone according to the face tracking results. +// Ajust the position by calculating the offset from the detected centered square. +// Then apply this offset as a delta vector and convert it to flight control commands. +void MainWindow::on_faceoffset(QVector3D &offset) +{ + qDebug() << "Offset: " << offset; + + if (offset.x() >= -90 && offset.x() <= 90 && offset.x() != 0) { + if (offset.x() < 0) { + controller->add_command("cw 10"); + controller->flush(); + drone_rotation += 10; + } else if (offset.x() > 0) { + controller->add_command("ccw 10"); + controller->flush(); + drone_rotation -= 10; + } + } + + if (offset.y() >= -70 && offset.y() <= 70 && offset.y() != -30) { + if (offset.y() < 0) { + controller->add_command("up 20"); + controller->flush(); + } else if (offset.y() > 0) { + controller->add_command("down 20"); + controller->flush(); + } + } + + if (offset.z() >= 15000 && offset.z() <= 30000 && offset.z() != -30) { + if (offset.z() < 15000) { + controller->add_command("forward 20"); + controller->flush(); + + } else if (offset.z() > 30000) { + controller->add_command("back 20"); + controller->flush(); + } + } +} + void MainWindow::on_button_start_recording_clicked() { if (video_recorder) { @@ -596,3 +673,38 @@ void MainWindow::on_splitter_splitterMoved(int pos, int index) }); } + +void MainWindow::on_button_add_waypoint_clicked() +{ + auto item = new QListWidgetItem("Point_" + QString::number(waypoints.size() + 1)); + waypoints.push_back(item); + ui->waypoints_list->addItem(item); +} + + +void MainWindow::on_button_remove_waypoint_clicked() +{ + for (auto item : ui->waypoints_list->selectedItems()) { + for (size_t i = 0; i < waypoints.size(); ++i) { + if (waypoints.at(i) == item) { + delete waypoints.at(i); + waypoints.remove(i); + } + } + } + + qDeleteAll(ui->waypoints_list->selectedItems()); + + if (ui->waypoints_list->count() > 0) + ui->waypoints_list->item(0)->setSelected(true); +} + +void MainWindow::on_waypoints_list_itemSelectionChanged() +{ + if (ui->waypoints_list->selectedItems().size() > 0) + ui->button_remove_waypoint->setDisabled(false); + else { + ui->button_remove_waypoint->setDisabled(true); + } +} +