DroneDeviceAzureIoTFullDemo.py

DroneDeviceAzureIoTFullDemo.py

# Copyright (c) 2020 
# Author: Bruno Capuano
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
#

# -----------------------------------------------
# 70 AZURE IOT HUB Sample
# -----------------------------------------------

import socket
import time
import threading
import iotc
import provision_service
import asyncio
import json
import datetime

from drone_device import Drone_Device

# -----------------------------------------------
# RECEIVE DATA FUNCTIONS
# -----------------------------------------------

def receiveData():
    global response, clientSocket
    while True:
        try:
            response, _ = clientSocket.recvfrom(1024)
        except:
            break

def readStates():
    global battery, agx, agy, agz, temph, templ
    global response, response_state, clientSocket, stateSocket, address

    while True:
        try:
            response_state, _ = stateSocket.recvfrom(256)
            if response_state != 'ok':
                response_state = response_state.decode('ASCII')
                list = response_state.replace(';', ':').split(':')
                battery = int(list[21])
                agx     = float(list[27])
                agy     = float(list[29])
                agz     = float(list[31])
                temph   = int(list[15])
                templ   = int(list[13])                
        # except:
        #     break
        except Exception as e:
            print(f'exc: {e}')
            pass

# -----------------------------------------------
# SEND COMMAND  FUNCTIONS
# -----------------------------------------------

def sendCommand(command):
    global response, clientSocket, address
    timestamp = int(time.time() * 1000)

    clientSocket.sendto(command.encode('utf-8'), address)

    while response is None:
        if (time.time() * 1000) - timestamp > 5 * 1000:
            return False

    return response


def sendReadCommand(command):
    global response
    response = sendCommand(command)
    try:
        response = str(response)
    except:
        pass
    return response

def sendControlCommand(command):
    global response, response_state, clientSocket, stateSocket, address
    response = None
    for i in range(0, 5):
        response = sendCommand(command)
        if response == 'OK' or response == 'ok':
            return True
    return False


# -----------------------------------------------
# AZURE IOT CENTRAL
# -----------------------------------------------

async def init_drone_AzureIoT():
    global drone

    iothub    = ""
    scope     = "Azure IoT Central Device Connect Scope ID goes here"
    device_id = "Azure IoT Central Device Connect Device ID goes here"
    key       = "Azure IoT Central Device Connect KEY goes here"
    drone = Drone_Device(scope, device_id, key)
    await drone.init_azureIoT()

# -----------------------------------------------
# 35 CAMERA
# APP DISPLAY CAMERA WITH OPENCV and FPS
# -----------------------------------------------

async def main():
    global battery, agx, agy, agz, temph, templ
    global response, response_state, clientSocket, stateSocket, address
    global drone

    await init_drone_AzureIoT()

    # CONNECTION TO THE DRONE
    
    # connection info
    UDP_IP = '192.168.10.1'
    UDP_PORT = 8889
    last_received_command = time.time()
    STATE_UDP_PORT = 8890

    address = (UDP_IP, UDP_PORT)
    response = None
    response_state = None

    clientSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    clientSocket.bind(('', UDP_PORT))
    stateSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    stateSocket.bind(('', STATE_UDP_PORT))

    # LISTENER THREADS

    # start threads
    recThread = threading.Thread(target=receiveData)
    recThread.daemon = True
    recThread.start()

    stateThread = threading.Thread(target=readStates)
    stateThread.daemon = True
    stateThread.start()

    # START DRONE CONNECTION

    # connect to drone
    response = sendControlCommand("command")
    print(f'command response: {response}')
    response = sendControlCommand("streamon")
    print(f'streamon response: {response}')

# MAIN APP

# drone information
battery = 0
agx     = 0
agy     = 0
agz     = 0
temph   = 0
templ   = 0

i = 0
while i < 300:
    i = i + 1
    sendReadCommand('battery?')

    await drone.send_telemetry(agx, agy, agz)

    if (i % 10) == 0:
        await drone.send_properties(temph, templ, battery)

    time.sleep(1)

if __name__ == "__main__":
    asyncio.run(main())