ckung0400
Published © GPL3+

A Multifunctional Community Helper Drone

A HoverGames Drone with NavQ Companion Computer to Fight COVID-19

IntermediateFull instructions provided5 days748

Things used in this project

Hardware components

KIT-HGDRONEK66
NXP KIT-HGDRONEK66
×1
Arduino UNO
Arduino UNO
×1
NXP NavQ Linux Computer
×1
RunCam wi-fi camera
×1
Uctronics 128x64 Yellow Blue OLED Display
×1
MLX906 IR Temperature Sensor
×1
Darlington High Power Transistor
Darlington High Power Transistor
×1
SG90 Micro-servo motor
SG90 Micro-servo motor
×1
Ultra Bright 12V LEDs
×1
Speaker, 2", 8 Ohm
×1
Ultrasonic Sensor - HC-SR04 (Generic)
Ultrasonic Sensor - HC-SR04 (Generic)
×1

Software apps and online services

OpenCV
OpenCV
Arduino IDE
Arduino IDE
Onshape Cloud base CAD
Gstreamer
Xlsxwriter
Matplotlib
QGroundControl
PX4 QGroundControl
PX4 Flight Review
MAVSDK
PX4 MAVSDK
VideoPad Video Editor

Story

Read more

Schematics

NavQ and Arduino UNO Serial Wiring

NavQ to Arduino UNO I2C Wiring

Arduino UNO - OLED Display, IR Sensor, Speaker, Servo, and LED Wiring

Code

NavQ and Arduino UNO Serial Communication

Python
#!/usr/bin/env python3

# Import libraries *************************************************
import cv2 as cv
import numpy as np
import time
import serial
import xlsxwriter
from matplotlib import pyplot as plt

# Setup serial communication with Arduino UNO **********************
if __name__ == '__main__':
    ser = serial.Serial('/dev/ttyACM0', 9600, timeout=60)
    ser.flush()

# Open temperature data file ***************************************
filename="temperature.txt"
myfile=open(filename,'w')

# Open Excel workbook for write ************************************
workbook=xlsxwriter.Workbook('demo.xlsx')
workbook.formats[0].set_font_size(18)
worksheet=workbook.add_worksheet()
worksheet.set_column('A:Z',36)
worksheet.set_default_row(145)


# Ask UNO to start the service *************************************
ser.write(b"Start the service\n")

# Capture 5 pictures ***********************************************
cap=cv.VideoCapture(0)
ret, frame=cap.read()
cv.imwrite('1.png', frame)
time.sleep(1)
ret, frame=cap.read()
cv.imwrite('2.png', frame)
time.sleep(1)
ret, frame=cap.read()
cv.imwrite('3.png', frame)
time.sleep(1)
ret, frame=cap.read()
cv.imwrite('4.png', frame)
time.sleep(1)
ret, frame=cap.read()
cv.imwrite('5.png', frame)
time.sleep(1)
cap.release()

# Read ID template *************************************************
template = cv.imread('ID2.png',0)
w, h = template.shape[::-1]

# Preprocess picture ***********************************************
img=cv.imread('1.png')
r640=cv.resize(img,(640,480))
cv.imwrite('IDimg.png',r640)
img_rgb = cv.imread('IDimg.png')
img_gray = cv.cvtColor(img_rgb, cv.COLOR_BGR2GRAY)

# Check ID *********************************************************
count=0
res = cv.matchTemplate(img_gray,template,cv.TM_CCOEFF_NORMED)
threshold = 0.41
loc = np.where( res >= threshold)
for pt in zip(*loc[::-1]):
    count += 1
    cv.rectangle(img_rgb, pt, (pt[0] + w, pt[1] + h), (0,255,0), 2)
cv.imwrite('r1.png',img_rgb)
print(count)

# Preprocess picture ***********************************************
img=cv.imread('2.png')
r640=cv.resize(img,(640,480))
cv.imwrite('IDimg.png',r640)
img_rgb = cv.imread('IDimg.png')
img_gray = cv.cvtColor(img_rgb, cv.COLOR_BGR2GRAY)

# Check ID *********************************************************
res = cv.matchTemplate(img_gray,template,cv.TM_CCOEFF_NORMED)
threshold = 0.41
loc = np.where( res >= threshold)
for pt in zip(*loc[::-1]):
    count += 1
    cv.rectangle(img_rgb, pt, (pt[0] + w, pt[1] + h), (0,255,0), 2)
cv.imwrite('r2.png',img_rgb)

print(count)

# Preprocess picture ***********************************************
img=cv.imread('3.png')
r640=cv.resize(img,(640,480))
cv.imwrite('IDimg.png',r640)
img_rgb = cv.imread('IDimg.png')
img_gray = cv.cvtColor(img_rgb, cv.COLOR_BGR2GRAY)

# Check ID *********************************************************
res = cv.matchTemplate(img_gray,template,cv.TM_CCOEFF_NORMED)
threshold = 0.41
loc = np.where( res >= threshold)
for pt in zip(*loc[::-1]):
    count += 1
    cv.rectangle(img_rgb, pt, (pt[0] + w, pt[1] + h), (0,255,0), 2)
cv.imwrite('r3.png',img_rgb)
print(count)

# Preprocess picture ***********************************************
img=cv.imread('4.png')
r640=cv.resize(img,(640,480))
cv.imwrite('IDimg.png',r640)
img_rgb = cv.imread('IDimg.png')
img_gray = cv.cvtColor(img_rgb, cv.COLOR_BGR2GRAY)

# Check ID *********************************************************
res = cv.matchTemplate(img_gray,template,cv.TM_CCOEFF_NORMED)
threshold = 0.41
loc = np.where( res >= threshold)
for pt in zip(*loc[::-1]):
    count += 1
    cv.rectangle(img_rgb, pt, (pt[0] + w, pt[1] + h), (0,255,0), 2)
cv.imwrite('r4.png',img_rgb)
print(count)

# Preprocess picture ***********************************************
img=cv.imread('5.png')
r640=cv.resize(img,(640,480))
cv.imwrite('IDimg.png',r640)
img_rgb = cv.imread('IDimg.png')
img_gray = cv.cvtColor(img_rgb, cv.COLOR_BGR2GRAY)

# Check ID *********************************************************
res = cv.matchTemplate(img_gray,template,cv.TM_CCOEFF_NORMED)
threshold = 0.41
loc = np.where( res >= threshold)
for pt in zip(*loc[::-1]):
    count += 1
    cv.rectangle(img_rgb, pt, (pt[0] + w, pt[1] + h), (0,255,0), 2)
cv.imwrite('r5.png',img_rgb)
print(count)

if count>0:
    print("pass")

# Read and write temperature data from UNO *************************
i=0
while True: 
    i=i+1
    ser.write(b"Start the service\n")
    line = ser.readline().decode('utf-8').rstrip()
    print(line)
    myfile.write(line)
    myfile.write("\n")
    if i==50:
        worksheet.write('D1',line)
    if i>80:
        break
myfile.close()

# Resize ID pictures to fit in Excel cells *************************
img=cv.imread('r1.png')
icon=cv.resize(img,(256,192))
cv.imwrite('r1icon.png',icon)

img=cv.imread('r2.png')
icon=cv.resize(img,(256,192))
cv.imwrite('r2icon.png',icon)

img=cv.imread('r3.png')
icon=cv.resize(img,(256,192))
cv.imwrite('r3icon.png',icon)

img=cv.imread('r4.png')
icon=cv.resize(img,(256,192))
cv.imwrite('r4icon.png',icon)

img=cv.imread('r5.png')
icon=cv.resize(img,(256,192))
cv.imwrite('r5icon.png',icon)

img=cv.imread('ID2.png')
icon=cv.resize(img,(256,192))
cv.imwrite('ID2icon.png',icon)

img=cv.imread('m2.png')
icon=cv.resize(img,(256,192))
cv.imwrite('m2icon.png',icon)

seconds=time.time()-18000
local_time=time.ctime(seconds)

# Write the Excel worksheet ****************************************
worksheet.write('B1',local_time)
worksheet.insert_image('C1','m2icon.png')
worksheet.insert_image('A1','ID2icon.png')
worksheet.insert_image('E1','r1icon.png')
worksheet.insert_image('F1','r2icon.png')
worksheet.insert_image('G1','r3icon.png')
worksheet.insert_image('H1','r4icon.png')
worksheet.insert_image('I1','r5icon.png')
workbook.close()

Generate QR Code Picture Using OpenCV

Python
# import QR code module
import qrcode

# input data
data="Jason Martin,age 39,301-123-4567,j.martin@gmail.com,Time Square-New York-12/1/20,Rockville Town Center-Maryland-12/20/20"

# enter filename
file="qrcode.jpg"

# generate image with data
image=qrcode.make(data)

# save the image to file
image.save(file)

Arduino <-> NavQ Serial Link

Arduino
#include <Wire.h> //I2C

#include <Adafruit_MLX90614.h> // IR temperature sensor ***********************************************************
Adafruit_MLX90614 mlx = Adafruit_MLX90614();

#include <Adafruit_SSD1306.h> // OLED display *********************************************************************
#define SCREEN_WIDTH 128 
#define SCREEN_HEIGHT 64 
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

void setup() 
{   
    Serial.begin(9600); // start serial communication with NavQ ***************************************************
    pinMode(13, OUTPUT); // green light ***************************************************************************
    pinMode(12, OUTPUT); //red light ******************************************************************************
    pinMode(11, OUTPUT); //servo **********************************************************************************
    pinMode(8, OUTPUT); //speaker *********************************************************************************
    dclose();
    display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
    display.clearDisplay();
    display.setRotation(2);
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 5);
    display.println("Initializing Temp");
    display.display();
    delay(250);
    display.clearDisplay();
    mlx.begin(); 
    showID();
}

void loop()
{     
    if (Serial.available()>0)
    {
    delay(5000);
    String data=Serial.readStringUntil('\n');
    Serial.print("NavQ sent me(UNO) the command: ");
    Serial.println(data);
    Serial.println("Service started");
    temp();
    delay(5000);
    thanks();
    mask();
    c19();
    //ATB();
    seeU();
    Serial.println("Service finished");
    Serial.end();
    delay(10000000);
    }
 }

 void temp(){// Display message and take temperature **************************************************************
    display.clearDisplay();
    display.setTextSize(2); // Draw 2X-scale text
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(10, 0);
    display.println(F("Place")); 
    display.println(F(" index")); 
    display.println(F("  finger")); 
    display.println(F(" on sensor")); 
    display.display();      // Show initial text
    delay(3000);
    for(int i=1;i<=100;i++){
      display.clearDisplay();
      display.setTextSize(2);
      display.setCursor(0, 30);
      display.print(mlx.readObjectTempF());
      Serial.println(mlx.readObjectTempF());// send temp data to NavQ *********************************************
      display.setCursor(70, 30);
      display.print("F");
      display.display(); 
      }
      Serial.println("Temperature taken");
    }

void mask(){ // Display message and open/close door ***************************************************************
  display.clearDisplay();
  display.setTextSize(2); // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 0);
  display.println(F("Please")); 
  display.println(F(" take"));
  display.println(F("  your"));
  display.println(F("   mask"));
  display.display();      // Show initial text
  delay(2000);
  grn();
  dopen();
  delay(5000);
  note(392,4);//G4
  note(330,2);//E4
  dclose();
  Serial.println("Mask delivered");
  }

void thanks(){ 
  display.clearDisplay();
  display.setTextSize(3); // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 0);
  display.println(F("Thank  You!"));  
  display.display();      // Show initial text
  }

void seeU(){
  display.clearDisplay();
  display.setTextSize(3); // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 0);
  display.println(F("See     You!"));  
  display.display();      // Show initial text
  }

void showID(){
  display.clearDisplay();
  display.setTextSize(2); // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 0);
  display.println(F("Please")); 
  display.println(F("   show  "));
  display.println(F(""));
  display.println(F(" your ID"));
  display.display();      // Show initial text
  }
 
void c19(){ //Sing Covid-19 Go Away song and display lyrics *******************************************************
  line1();
  line2();
  line3();
  line4();
  }

void line1() {
  display.clearDisplay();
  display.setTextSize(2); // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 0);
  display.println(F(""));
  display.println(F("Covid-19"));
  display.println(F(""));
  display.println(F(" Go Away!"));
  display.display();      // Show initial text
  delay(100);
  note(392,2);//G4
  note(330,2);//E4
  note(392,4);//G4
  note(392,4);//G4
  note(330,2);//E4 
  }

void line2() {
  display.clearDisplay();
  display.setTextSize(2); // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 0);
  display.println(F("Never "));
  display.println(F(" comeback"));
  display.println(F(""));
  display.println(F("  any day"));
  display.display();      // Show initial text
  delay(100);
  note(392,4);//G4
  note(392,4);//G4
  note(330,4);//E4
  note(440,4);//A4
  note(392,4);//G4
  note(392,4);//G4
  note(330,2);//E4
  }

void line3() {
  display.clearDisplay();
  display.setTextSize(2); // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 0);
  display.println(F("All the "));
  display.println(F(" children"));
  display.println(F("  want to"));
  display.println(F("   play"));
  display.display();      // Show initial text
  delay(100);
  note(349,4);//F4
  note(349,4);//F4
  note(294,4);//D4
  note(294,4);//D4
  note(349,4);//F4
  note(349,4);//F4
  note(294,2);//D4
  }

void line4() {
  display.clearDisplay();
  display.setTextSize(2); // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 0);
  display.println(F("Covid "));
  display.println(F(" nineteen"));
  display.println(F("   "));
  display.println(F("  go away"));
  display.display();      // Show initial text
  delay(100);
  note(392,4);//G4
  note(349,4);//F4
  note(330,4);//E4
  note(294,4);//D4
  note(330,4);//E4
  note(262,4);//C4
  note(262,2);//C4
  }

void red(){ // Blink red LED **************************************************************************************
 digitalWrite(12,1);
 delay(500);
 digitalWrite(12,0);
}

void grn(){ // Blink green LED ************************************************************************************
   for(int i=1;i<=5;i++){
 	    digitalWrite(13,1);
 	    delay(500);
  	  digitalWrite(13,0);
 	    delay(500);
	    }
	  }


void ATB() // Play America The Beautiful **************************************************************************
{
 note(392,4);//G4
 note(392,3);//G4
 note(330,8);//E4
 note(330,4);//E4
 note(392,4);//G4
 note(392,3);//G4
 note(294,8);//D4
 note(294,4);//D4
 note(330,4);//E4
 note(349,4);//F4
 note(392,4);//G4
 note(440,4);//A4
 note(494,4);//B4
 note(392,2);//G4
 note(0,4);//rest
 note(392,4);//G4
 note(392,3);//G4
 note(330,8);//E4
 note(330,4);//E4
 note(392,4);//G4
 note(392,3);//G4
 note(294,8);//D4
 note(294,4);//D4
 note(587,4);//D5
 note(523,4);//C5
 note(587,4);//D5
 note(659,4);//E5
 note(440,4);//A4
 note(587,2);//D5
 note(0,4);//rest
 note(392,4);//G4
 note(659,3);//E5
 note(659,8);//E5
 note(587,4);//D5
 note(523,4);//C5
 note(523,3);//C5
 note(494,8);//B4
 note(494,4);//B4
 note(523,4);//C5
 note(587,4);//D5
 note(494,4);//B4
 note(440,4);//A4
 note(392,4);//G4
 note(523,2);//C5
 note(0,4);//rest
 note(523,4);//C5
 note(523,3);//C5
 note(440,8);//A4
 note(440,4);//A4
 note(523,4);//C5
 note(523,3);//C5
 note(392,8);//G4
 note(392,4);//G4
 note(392,4);//G4
 note(440,4);//A4
 note(523,4);//C5
 note(392,4);//G4
 note(587,4);//D5
 note(523,1);//C5
 note(0,1);//rest
}

void note(int pit,int dur){ // Play a note ************************************************************************
  if(pit==0)
  {
    delay(1000/dur);
  }
  else
  {
    for (int i=0;i<pit/dur;i++)
    {
      analogWrite(8,255);
      delayMicroseconds(500000/pit);
      analogWrite(8,0);
      delayMicroseconds(500000/pit);
    }
  }
  delay(125);
  }

void dopen(){ // Open payload door ********************************************************************************
  for (int i=0;i<15;i++){
    digitalWrite(11,1);
    delayMicroseconds(1500);
    digitalWrite(11,0);
    delay(20);
    }
  }

void dclose(){ // Close payload door ******************************************************************************
  for (int i=0;i<15;i++){
    digitalWrite(11,1);
    delayMicroseconds(500);
    digitalWrite(11,0);
    delay(20);
    }
  }

A mission control program using MAVSDK

C/C++
// ONE STOP MISSION USING MAVSDK LIBRARY *******************************************************
// takeoff -> waypoint -> land -> RTL
//
// Author: Julian Oes <julian@oes.ch>
// modified by Charles Kung <ckung0400@yahoo.com> on 1/24/2021

#include <chrono>
#include <cstdint>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <iostream>
#include <thread>

using namespace mavsdk;
using namespace std::this_thread;
using namespace std::chrono;

#define ERROR_CONSOLE_TEXT "\033[31m" // Turn text on console red
#define TELEMETRY_CONSOLE_TEXT "\033[34m" // Turn text on console blue
#define NORMAL_CONSOLE_TEXT "\033[0m" // Restore normal console colour

void usage(std::string bin_name)
{
    std::cout << NORMAL_CONSOLE_TEXT << "Usage : " << bin_name << " <connection_url>" << std::endl
              << "Connection URL format should be :" << std::endl
              << " For TCP : tcp://[server_host][:server_port]" << std::endl
              << " For UDP : udp://[bind_host][:bind_port]" << std::endl
              << " For Serial : serial:///path/to/serial/dev[:baudrate]" << std::endl
              << "For example, to connect to the simulator use URL: udp://:14540" << std::endl;
}

void component_discovered(ComponentType component_type)
{
    std::cout << NORMAL_CONSOLE_TEXT << "Discovered a component with type "
              << unsigned(component_type) << std::endl;
}
// ************************************************************************************************
int main(int argc, char** argv)
{
    Mavsdk mavsdk;
    std::string connection_url;
    ConnectionResult connection_result;

    bool discovered_system = false;
    if (argc == 2) {
        connection_url = argv[1];
        connection_result = mavsdk.add_any_connection(connection_url);
    } else {
        usage(argv[0]);
        return 1;
    }

    if (connection_result != ConnectionResult::Success) {
        std::cout << ERROR_CONSOLE_TEXT << "Connection failed: " << connection_result
                  << NORMAL_CONSOLE_TEXT << std::endl;
        return 1;
    }

    std::cout << "Waiting to discover system..." << std::endl;
    mavsdk.subscribe_on_new_system([&mavsdk, &discovered_system]() {
        const auto system = mavsdk.systems().at(0);

        if (system->is_connected()) {
            std::cout << "Discovered system" << std::endl;
            discovered_system = true;
        }
    });

    // We usually receive heartbeats at 1Hz, therefore we should find a system after around 2
    // seconds.
    sleep_for(seconds(2));

    if (!discovered_system) {
        std::cout << ERROR_CONSOLE_TEXT << "No system found, exiting." << NORMAL_CONSOLE_TEXT
                  << std::endl;
        return 1;
    }

    const auto system = mavsdk.systems().at(0);

    // Register a callback so we get told when components (camera, gimbal) etc
    // are found.
    system->register_component_discovered_callback(component_discovered);

    auto telemetry = Telemetry{system};
    auto action = Action{system};

    // We want to listen to the altitude of the drone at 1 Hz.
    const Telemetry::Result set_rate_result = telemetry.set_rate_position(1.0);
    if (set_rate_result != Telemetry::Result::Success) {
        std::cout << ERROR_CONSOLE_TEXT << "Setting rate failed:" << set_rate_result
                  << NORMAL_CONSOLE_TEXT << std::endl;
        return 1;
    }

    // Set up callback to monitor altitude while the vehicle is in flight
    telemetry.subscribe_position([](Telemetry::Position position) {
        std::cout << TELEMETRY_CONSOLE_TEXT // set to blue
                  << "Altitude: " << position.relative_altitude_m << " m"
                  << NORMAL_CONSOLE_TEXT // set to default color again
                  << std::endl;
    });

    // Check if vehicle is ready to arm
    while (telemetry.health_all_ok() != true) {
        std::cout << "Vehicle is getting ready to arm" << std::endl;
        sleep_for(seconds(1));
    }

    // Arm vehicle ***********************************************************************************
    std::cout << "Arming..." << std::endl;
    const Action::Result arm_result = action.arm();

    if (arm_result != Action::Result::Success) {
        std::cout << ERROR_CONSOLE_TEXT << "Arming failed:" << arm_result << NORMAL_CONSOLE_TEXT
                  << std::endl;
        return 1;
    }

    // Take off **************************************************************************************
    std::cout << "Taking off..." << std::endl;
    const Action::Result takeoff_result = action.takeoff();
    if (takeoff_result != Action::Result::Success) {
        std::cout << ERROR_CONSOLE_TEXT << "Takeoff failed:" << takeoff_result
                  << NORMAL_CONSOLE_TEXT << std::endl;
        return 1;
    }

    // Let it hover for a bit before landing again.
    sleep_for(seconds(10));


	// goto_location *********************************************************************************
    std::cout << "goto Waypoint..." << std::endl;
    const Action::Result goto_location_result = action.goto_location(39.071682,-77.216449,100.0f,0.0f);
    if (goto_location_result != Action::Result::Success) {
        std::cout << ERROR_CONSOLE_TEXT << "goto Waypoint failed:" << goto_location_result
                  << NORMAL_CONSOLE_TEXT << std::endl;
        return 1;
    	}

	// Landing ***************************************************************************************
    std::cout << "Landing..." << std::endl;
    const Action::Result land_result = action.land();
    if (land_result != Action::Result::Success) {
        std::cout << ERROR_CONSOLE_TEXT << "Land failed:" << land_result << NORMAL_CONSOLE_TEXT
                  << std::endl;
        return 1;
    }

    // Check if vehicle is still in air
    while (telemetry.in_air()) {
        std::cout << "Vehicle is landing..." << std::endl;
        sleep_for(seconds(1));
    }
    std::cout << "Landed!" << std::endl;   
	
	// RTL *******************************************************************************************
	{
        // Mission complete. Command RTL to go home.
        std::cout << "Commanding RTL..." << std::endl;
        const Action::Result result = action.return_to_launch();
        if (result != Action::Result::Success) {
            std::cout << "Failed to command RTL (" << result << ")" << std::endl;
        } else {
            std::cout << "Commanded RTL." << std::endl;
        }
    	}
	// We are relying on auto-disarming but let's keep watching the telemetry for a bit longer.
    	sleep_for(seconds(3));
    	std::cout << "Finished..." << std::endl;

    	return 0;

}

Credits

ckung0400

ckung0400

2 projects • 0 followers
A mechanical engineer, a teacher, an R/C enthusiast, and a robot and drone lover.

Comments