Tools: added script to convert old plane PIDs

Tools/scripts/convert_plane_PID.py

@@ -0,0 +1,115 @@
+#!/usr/bin/env python
+'''
+convert fixed wing PIDs in a parameter file from old system to ACPID system
+'''
+
+import os
+import re
+
+found_line = False
+changed_param = False
+
+def process_file(fname):
+    global found_line
+    global changed_param
+
+    found_line = False
+    changed_param = False
+
+    print("Processing %s" % fname)
+    lines = open(fname).readlines()
+    for i in range(len(lines)):
+        lines[i] = lines[i].rstrip()
+    while lines[len(lines)-1].strip() == "":
+        lines = lines[:-1]
+
+    def find_line(pname):
+        global found_line
+        for i in range(len(lines)):
+            if lines[i].startswith(pname) and not lines[i][len(pname)].isalpha():
+                found_line = True
+                return i
+        return None
+
+    def find_param(pname, dvalue):
+        i = find_line(pname)
+        if i is None:
+            return dvalue
+        s = lines[i].replace(","," ")
+        a = s.split()
+        return float(a[1])
+
+    def set_param(pname, value, old_name):
+        global changed_param
+        i = find_line(pname)
+        if i is None:
+            i = find_line(old_name)
+        if i is None:
+            changed_param = True
+            lines.append("%s %f" % (pname, value))
+            return
+        # maintain separator if possible
+        sep = " "
+        m = re.match("[A-Z_0-9]+([\s,]+)[0-9.-]+", lines[i])
+        if m is not None:
+            sep = m.group(1)
+        changed_param = True
+        lines[i] = "%s%s%f" % (pname, sep, value)
+
+    print("Converting pitch")
+    FF = find_param("PTCH2SRV_FF", 0)
+    P = find_param("PTCH2SRV_P", 1.0)
+    I = find_param("PTCH2SRV_I", 0.3)
+    D = find_param("PTCH2SRV_D", 0.04)
+    IMAX = find_param("PTCH2SRV_IMAX", 3000)
+    TCONST = find_param("PTCH2SRV_TCONST", 0.5)
+
+    if FF <= 0:
+        I = max(I,0.3)
+
+    kp_ff = max((P - I * TCONST) * TCONST  - D, 0)
+    if found_line:
+        set_param("PTCH_RATE_FF", FF + kp_ff, "PTCH2SRV_FF")
+        set_param("PTCH_RATE_P", D, "PTCH2SRV_P")
+        set_param("PTCH_RATE_I", I * TCONST, "PTCH2SRV_I")
+        set_param("PTCH_RATE_D", 0, "PTCH2SRV_D")
+        set_param("PTCH_RATE_IMAX", IMAX/4500.0, "PTCH2SRV_IMAX")
+
+    found_line = False
+
+    print("Converting roll")
+    FF = find_param("RLL2SRV_FF", 0)
+    P = find_param("RLL2SRV_P", 1.0)
+    I = find_param("RLL2SRV_I", 0.3)
+    D = find_param("RLL2SRV_D", 0.08)
+    IMAX = find_param("RLL2SRV_IMAX", 3000)
+    TCONST = find_param("RLL2SRV_TCONST", 0.5)
+
+    kp_ff = max((P - I * TCONST) * TCONST  - D, 0)
+    if found_line:
+        set_param("RLL_RATE_FF", FF + kp_ff, "RLL2SRV_FF")
+        set_param("RLL_RATE_P", D, "RLL2SRV_P")
+        set_param("RLL_RATE_I", I * TCONST, "RLL2SRV_I")
+        set_param("RLL_RATE_D", 0, "RLL2SRV_D")
+        set_param("RLL_RATE_IMAX", IMAX/4500.0, "RLL2SRV_IMAX")
+
+    if not changed_param:
+        print("No fixed wing PID params")
+        return
+
+    print("Writing")
+    tfile = fname + ".tmp"
+    f = open(tfile,"w")
+    for i in range(len(lines)):
+        f.write("%s\n" % lines[i])
+    f.close()
+    os.rename(tfile, fname)
+
+import argparse
+parser = argparse.ArgumentParser(description='convert plane PIDs from old to new system')
+
+parser.add_argument('param_file', nargs='+')
+args = parser.parse_args()
+
+for f in args.param_file:
+    process_file(f)