#### 256 lines 9.3 KiB Python Raw Permalink Blame History

 ```#!/usr/bin/python3 ``` ``` ``` ```# Convert QGroundControl survey waypoints to DJI GO 4 waypoints 2.0 ``` ```# ``` ```# Solely for the creation of terrain survey mapping ``` ```# at a fixed altitude. ``` ``` ``` ```import sys ``` ```import os ``` ```import shutil ``` ```import argparse ``` ```import time ``` ```import math ``` ```import random ``` ```import sqlite3 ``` ```import json ``` ``` ``` ``` ``` ```####### Globals ################################################################ ``` ``` ``` ```mission_table = 'dji_pilot_dji_groundstation_waypoint2_model_WayPoint2MissionDBModel' ``` ```waypoint_table = 'dji_pilot_dji_groundstation_waypoint2_model_WayPoint2MissionDBModel\$WayPoint2DBPoint' ``` ``` ``` ``` ``` ```####### Helper Function(s) ##################################################### ``` ``` ``` ```def haversine(coord1, coord2): ``` ``` R = 6371000 #6372800 # Earth radius in meters [looks like DJI GO 4 uses 6371000] ``` ``` lat1, lon1 = coord1 ``` ``` lat2, lon2 = coord2 ``` ``` ``` ``` phi1, phi2 = math.radians(lat1), math.radians(lat2) ``` ``` dphi = math.radians(lat2 - lat1) ``` ``` dlambda = math.radians(lon2 - lon1) ``` ``` ``` ``` a = math.sin(dphi/2)**2 + \ ``` ``` math.cos(phi1) * math.cos(phi2) * math.sin(dlambda/2)**2 ``` ``` ``` ``` return 2 * R * math.atan2(math.sqrt(a), math.sqrt(1 - a)) ``` ``` ``` ``` ``` ```################################################################################ ``` ``` ``` ``` ``` ```# TODO: Confirm that 'complexItemType' = "survey" ``` ```# TODO: Also update 'way_point_2_cache.db' ``` ```# - delete existing cached mission, and its' waypoints ``` ```# - insert the same new data as with 'way_point_2.db' ``` ``` ``` ```parser = argparse.ArgumentParser(description='Convert QGroundControl survey waypoints to DJI GO 4 waypoints 2.0') ``` ``` ``` ```parser.add_argument('-d', '--dbfile', required=True, type=str, ``` ``` help='DJI GO 4 database file') ``` ```parser.add_argument('-i', '--infile', required=True, type=argparse.FileType('r'), ``` ``` help='JSON input file') ``` ```parser.add_argument('-a', '--altitude', type=float, ``` ``` help='Altitude above ground, in meters') ``` ```parser.add_argument('-f', '--finishaction', type=int, default=4, ``` ``` help='Act to perform upon mission completion [0, 1, 2, 3 or 4 - hover] (default: %(default)s)') ``` ```parser.add_argument('-n', '--name', type=str, ``` ``` help='Name for the mission (qgc2dji-survey_)') ``` ```parser.add_argument('-s', '--speed', type=float, ``` ``` help='Flight speed') ``` ``` ``` ```args = parser.parse_args() ``` ``` ``` ```if not os.path.isfile(args.dbfile): ``` ``` print("[ERROR] Database file does not exist!", file=sys.stderr) ``` ``` exit(2) ``` ``` ``` ```try: ``` ``` conn = sqlite3.connect(args.dbfile) ``` ```except sqlite3.Error as e: ``` ``` print("[ERROR] Could not connect to database!\n" + e.args[0], file=sys.stderr) ``` ``` exit(2) ``` ``` ``` ``` ``` ```# The only values we care about are inside "TransectStyleComlexItem" ``` ```items = json.loads(args.infile.read())['mission']['items'] ``` ``` ``` ```main_data = None ``` ```for item in items: ``` ``` if 'TransectStyleComplexItem' in item: ``` ``` main_data = item["TransectStyleComplexItem"] ``` ``` break ``` ```if not main_data: ``` ``` print("[ERROR] Required data not present in json!", file=sys.stderr) ``` ``` exit(2) ``` ``` ``` ```altitude = args.altitude ``` ```if not altitude: ``` ``` altitude = main_data["CameraCalc"]["DistanceToSurface"] ``` ``` ``` ```finishaction = args.finishaction ``` ``` ``` ```# If this is no set, it will be later on during parsing ``` ```flight_speed = args.speed ``` ```if not flight_speed: ``` ``` # First check if a value was set in the json ``` ``` for item in items: ``` ``` if 'command' in item and item['command'] == 178: ``` ``` # Reverse-engineered this location - could easily change in future QGroundControl releases ``` ``` flight_speed = item['params'][1] ``` ``` break ``` ```# Set an acceptable, but slow default if speed still hasn't been set ``` ```# TODO: Calculate this based on mission distance / photo count / 5s to get one photo per 5seconds ??? ``` ```if not flight_speed: ``` ``` flight_speed = 3.5 ``` ``` ``` ```mission_name = args.name ``` ```if not mission_name: ``` ``` timestr = time.strftime('%d%m%Y-%H%M%S') ``` ``` mission_name = "qgc2dji-survey_" + timestr ``` ``` ``` ```# DJI GO database stores time in milliseconds since epoch ``` ```update_time = int(round(time.time() * 1000)) ``` ``` ``` ``` ``` ```cur = conn.cursor() ``` ``` ``` ```cur.execute(f'SELECT missionId FROM {mission_table}') ``` ```existing_ids = cur.fetchall() ``` ``` ``` ```# Looking at existing database entries, it appears that 'missionId' is an arbitrary value ``` ```mission_id = random.randrange(10000,99999) ``` ```while (mission_id,) in existing_ids: ``` ``` print("[INFO] ID collision. Generating new ID.") ``` ``` mission_id = random.randrange(10000,99999) ``` ``` ``` ```waypoint_queue = [] ``` ``` ``` ```# Set default values for DJI GO database waypoint table ``` ```heading_type = 0 # Free ? ``` ```poi_index = -1 # None ? ``` ```heading = 0 # Look-ahead ? ``` ```action = 0 # Do nothing (as opposed to take photo, etc) ``` ```pitch = -90.0 # Point camera toward ground ``` ```radius = 2.0 # Some default ? Could be realted to 'arc' vs 'polyline' ``` ```speed = 0.0 # Use 'autoFlightSpeed' from 'mission_table' ``` ``` ``` ```# Prepare each waypoint for insert ``` ```waypoints = main_data['VisualTransectPoints'] ``` ```last_coord = () ``` ```curr_coord = () ``` ```accum_distance = 0 ``` ``` ``` ```# TEST DATA - Expected accumulated result: 1825.33557128906 ``` ```# Result initially was 1825.8512451494107 which should be close enough ``` ```# Result with "standard" Earth radius is 1825.3355327088402 which is closer still ``` ```# TODO: Test geopy module ? [see: https://janakiev.com/blog/gps-points-distance-python/] ``` ```#waypoints = [(-19.9914615912953,57.6010317688705), ``` ```# (-19.9906124706645,57.6018561685789), ``` ```# (-19.9907586941667,57.6021400677547), ``` ```# (-19.9917566018806,57.6012092059048), ``` ```# (-19.9920208284113,57.6013183979067), ``` ```# (-19.9908972216132,57.6023884794603), ``` ```# (-19.9910049650248,57.6026669190904), ``` ```# (-19.9923184032966,57.601438509184), ``` ```# (-19.9925544106552,57.6017169485915), ``` ```# (-19.9911050125569,57.6029944951315), ``` ```# (-19.9912281477626,57.6033111519851), ``` ```# (-19.9927904174512,57.6019353328111), ``` ```# (-19.9931793793255,57.6022154786425), ``` ```# (-19.9915212340617,57.6036721688515), ``` ```# (-19.9915956280736,57.6039779066684), ``` ```# (-19.9934298150556,57.6024819755698)] ``` ``` ``` ```count = 0 ``` ```for waypoint in waypoints: ``` ``` latitude = waypoint[0] ``` ``` longitude = waypoint[1] ``` ``` waypoint_id = count ``` ``` count = count + 1 ``` ``` ``` ``` waypoint_queue.append((heading_type, altitude, mission_id, poi_index, heading, latitude, action, pitch, radius, speed, longitude, waypoint_id)) ``` ``` ``` ``` if count > 1: ``` ``` curr_coord = (latitude, longitude) ``` ``` accum_distance = accum_distance + haversine(last_coord, curr_coord) ``` ``` last_coord = curr_coord ``` ``` else: ``` ``` last_coord = (latitude, longitude) ``` ``` ``` ``` ``` ```# Set default values for DJI GO database mission table ``` ```is_use_custom_direction = 0 ``` ```first_lon = 0.0 ``` ```first_lat = 0.0 ``` ```local = ''#None # becomes NULL in sqlite db ``` ```exit_mission_on_rc_lost = 0 # Keep flying ``` ```flight_path_mode = 0 # 'polyline' vs 'arc' ? ``` ```is_cache = 0 # ? ``` ```rotate_gimbal_pitch = 1 # ? ``` ```goto_first_waypoint_mode = 0 # ? ``` ```repeat_times = 1 # Only fly the mission once ``` ```max_flight_speed = 8.3 # (m/s) Based on ~30km/h being a good maximum ``` ```heading_mode = 0 # ? Different from 'heading_type' ? ``` ```is_enable_multi_poi = 0 ``` ```route_distance = accum_distance #'' #None # Can we get away with leaving this blank !? ``` ```point_count = len(waypoints) ``` ``` ``` ``` ``` ```print("\n======= MISSION DATA =======\n") ``` ```print(f"Mission name:\t\t{mission_name}") ``` ```print(f"Mission id:\t\t{mission_id}") ``` ```print(f"Max flight speed:\t{max_flight_speed} m/s") ``` ```print(f"Cruise speed:\t\t{flight_speed} m/s") ``` ```print(f"Altitude:\t\t{altitude} m") ``` ```print(f"Route distance:\t\t{route_distance} m") ``` ```print(f"Finished action:\t{finishaction}") ``` ```print("") ``` ``` ``` ```# TODO: Implement more robust regex version of the following ``` ```proceed = '' ``` ```while proceed not in ['y', 'Y', 'n', 'N']: ``` ``` proceed = input("Write out to database? [y/n] ") ``` ``` ``` ```if proceed == 'n' or proceed == 'N': ``` ``` print("[INFO] Exiting without updating database.") ``` ``` exit(1) ``` ``` ``` ``` ``` ```# Generate SQL INSERT for the mission ``` ```sql = (f"INSERT INTO {mission_table}" ``` ``` "(missionId, isUseCustomDirection, updateTime, finishedAction, firstLng, local, " ``` ``` "exitMissionOnRCSignalLost, flightPathMode, isCache, rotateGimbalPitch, " ``` ``` "gotoFirstWaypointMode, pointCount, repeatTimes, routDistance, firstLat, " # n.b. 'routDistance' is not a spelling mistake ``` ``` "missionName, maxFlightSpeed, headingMode, autoFlightSpeed, isEnableMultiPOI) " ``` ``` "VALUES (?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?)") ``` ```cur.execute(sql, (mission_id, is_use_custom_direction, update_time, finishaction, ``` ``` first_lon, local, exit_mission_on_rc_lost, flight_path_mode, ``` ``` is_cache, rotate_gimbal_pitch, goto_first_waypoint_mode, point_count, ``` ``` repeat_times, route_distance, first_lat, mission_name, max_flight_speed, ``` ``` heading_mode, flight_speed, is_enable_multi_poi)) ``` ``` ``` ```# SQL INSERT for all waypoints from QGroundControl ``` ```sql = (f"INSERT INTO {waypoint_table}" ``` ``` "(headingType, altitude, missionId, poiIndex, heading, latitude, action, " ``` ``` "pitch, radius, speed, longitude, myIndex) " ``` ``` "VALUES (?,?,?,?,?,?,?,?,?,?,?,?)") ``` ```cur.executemany(sql, waypoint_queue) ``` ``` ``` ``` ``` ```# TODO: Implement database backup as a commandline option ``` ```shutil.copy2(args.dbfile, args.dbfile + ".bak") ``` ``` ``` ``` ``` ```# Write changes back to the database ``` ```conn.commit() ``` ``` ``` ``` ``` ```# TODO: Determine if there is any other clean up ``` ``` ``` ``` ``` ```conn.close() ```