From 756813008ba405bef51b10ed93ca977ef7c404e7 Mon Sep 17 00:00:00 2001
From: PonomarevDA <ponomarevda96@gmail.com>
Date: Sun, 17 Mar 2024 18:52:04 +0300
Subject: [PATCH] configurator: rewrite in python and add config for
 px4_v1_14_0_beta_dronecan_vtol

---
 .../px4_v1_14_0_beta_dronecan_vtol.yaml       |  6 +++
 scripts/configurator.py                       | 52 +++++++++++++++++++
 scripts/docker.sh                             |  6 ++-
 3 files changed, 62 insertions(+), 2 deletions(-)
 create mode 100644 configs/vehicles/px4_v1_14_0_beta_dronecan_vtol.yaml
 create mode 100755 scripts/configurator.py

diff --git a/configs/vehicles/px4_v1_14_0_beta_dronecan_vtol.yaml b/configs/vehicles/px4_v1_14_0_beta_dronecan_vtol.yaml
new file mode 100644
index 0000000..62579da
--- /dev/null
+++ b/configs/vehicles/px4_v1_14_0_beta_dronecan_vtol.yaml
@@ -0,0 +1,6 @@
+firmware: https://github.com/ZilantRobotics/PX4-Autopilot/releases/download/v1.14.0-0.4.1-beta1/px4_fmu-v5_default.px4
+params:
+  - configs/px4/v1.14/quadcopter/airframe.yaml
+  - configs/px4/v1.14/quadcopter/dronecan.yaml
+  - configs/px4/dronecan.yaml
+  - configs/px4/common.yaml
diff --git a/scripts/configurator.py b/scripts/configurator.py
new file mode 100755
index 0000000..ca88970
--- /dev/null
+++ b/scripts/configurator.py
@@ -0,0 +1,52 @@
+#!/usr/bin/env python3
+"""
+Autopilot configurator:
+1. Download a firmware and upload to a target if it is specified in yaml file
+2. Configure autopilot parameters if it is specified in yaml file
+"""
+import os
+import subprocess
+import wget
+from argparse import ArgumentParser
+import yaml
+
+SCRIPTS_DIR = os.path.dirname(os.path.realpath(__file__))
+REPO_DIR = os.path.dirname(SCRIPTS_DIR)
+BINARY_OUTPUT_PATH = f"{REPO_DIR}/firmware.bin"
+
+def configure(yaml_path : str) -> None:
+    with open(yaml_path, 'r') as file:
+        config = yaml.safe_load(file)
+
+    flags = []
+
+    firmware = config.get('firmware')
+    if firmware is not None:
+        if os.path.exists(BINARY_OUTPUT_PATH):
+            os.remove(BINARY_OUTPUT_PATH)
+        wget.download(firmware, out=BINARY_OUTPUT_PATH)
+        flags += ['--firmware', BINARY_OUTPUT_PATH]
+
+    params = config.get('params')
+    if params is not None and len(params) > 0:
+        flags += ['-f', '--config']
+        for param in params:
+            flags.append(f"{REPO_DIR}/{param}")
+
+    if len(flags) > 0:
+        subprocess.run(["autopilot-configurator", "-v", *flags])
+        if os.path.exists(BINARY_OUTPUT_PATH):
+            os.remove(BINARY_OUTPUT_PATH)
+
+def configure_cli():
+    """
+    ArgumentParser wrapper under configure() function.
+    This function can be used in pyproject.toml
+    """
+    parser = ArgumentParser(description=__doc__)
+    parser.add_argument('path', help='Path to config .yaml file')
+    args = parser.parse_args()
+    configure(args.path)
+
+if __name__ == "__main__":
+    configure_cli()
diff --git a/scripts/docker.sh b/scripts/docker.sh
index 13d7a69..50a035c 100755
--- a/scripts/docker.sh
+++ b/scripts/docker.sh
@@ -39,6 +39,7 @@ Commands (with aliases):
 }
 
 SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
+REPOSITORY_DIR="$(dirname "$SCRIPT_DIR")"
 
 # Kill the container if sniffer is disconnected
 slcan_checker() {
@@ -163,10 +164,11 @@ dronecan_vtol_v1_14_0() {
     kill_all_related_containers
     setup_dronecan_hitl_config
     slcan_checker&
+    vehicle="px4_v1_14_0_beta_dronecan_vtol"
     if [[ $OPTIONS == "--force" ]]; then
-        ./configure.sh px4_v1_14_0_beta_dronecan_vtol
+        ${REPOSITORY_DIR}/scripts/configurator.py ${REPOSITORY_DIR}/configs/vehicles/${vehicle}.yaml
     fi
-    docker container run --rm $DOCKER_FLAGS $IMAGE_NAME ./scripts/run_sim.sh px4_v1_14_0_beta_dronecan_vtol
+    docker container run --rm $DOCKER_FLAGS $IMAGE_NAME ./scripts/run_sim.sh ${vehicle}
 }
 
 dronecan_quadrotor() {