CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Tools/scripts/convert_plane_PID.py
Views: 1798
1
#!/usr/bin/env python

2
'''

3
convert fixed wing PIDs in a parameter file from old system to ACPID system

4
'''

5


6
import os

7
import re

8


9
found_line = False

10
changed_param = False

11


12
def process_file(fname):

13
    global found_line

14
    global changed_param

15


16
    found_line = False

17
    changed_param = False

18


19
    print("Processing %s" % fname)

20
    lines = open(fname).readlines()

21
    for i in range(len(lines)):

22
        lines[i] = lines[i].rstrip()

23
    while lines[len(lines)-1].strip() == "":

24
        lines = lines[:-1]

25


26
    def find_line(pname):

27
        global found_line

28
        for i in range(len(lines)):

29
            if lines[i].startswith(pname) and not lines[i][len(pname)].isalpha():

30
                found_line = True

31
                return i

32
        return None

33


34
    def find_param(pname, dvalue):

35
        i = find_line(pname)

36
        if i is None:

37
            return dvalue

38
        s = lines[i].replace(","," ")

39
        a = s.split()

40
        return float(a[1])

41


42
    def set_param(pname, value, old_name):

43
        global changed_param

44
        i = find_line(pname)

45
        if i is None:

46
            i = find_line(old_name)

47
        if i is None:

48
            changed_param = True

49
            lines.append("%s %f" % (pname, value))

50
            return

51
        # maintain separator if possible

52
        sep = " "

53
        m = re.match("[A-Z_0-9]+([\s,]+)[0-9.-]+", lines[i])

54
        if m is not None:

55
            sep = m.group(1)

56
        changed_param = True

57
        lines[i] = "%s%s%f" % (pname, sep, value)

58


59
    print("Converting pitch")

60
    FF = find_param("PTCH2SRV_FF", 0)

61
    P = find_param("PTCH2SRV_P", 1.0)

62
    I = find_param("PTCH2SRV_I", 0.3)

63
    D = find_param("PTCH2SRV_D", 0.04)

64
    IMAX = find_param("PTCH2SRV_IMAX", 3000)

65
    TCONST = find_param("PTCH2SRV_TCONST", 0.5)

66


67
    if FF <= 0:

68
        I = max(I,0.3)

69


70
    kp_ff = max((P - I * TCONST) * TCONST  - D, 0)

71
    if found_line:

72
        set_param("PTCH_RATE_FF", FF + kp_ff, "PTCH2SRV_FF")

73
        set_param("PTCH_RATE_P", D, "PTCH2SRV_P")

74
        set_param("PTCH_RATE_I", I * TCONST, "PTCH2SRV_I")

75
        set_param("PTCH_RATE_D", 0, "PTCH2SRV_D")

76
        set_param("PTCH_RATE_IMAX", IMAX/4500.0, "PTCH2SRV_IMAX")

77


78
    found_line = False

79


80
    print("Converting roll")

81
    FF = find_param("RLL2SRV_FF", 0)

82
    P = find_param("RLL2SRV_P", 1.0)

83
    I = find_param("RLL2SRV_I", 0.3)

84
    D = find_param("RLL2SRV_D", 0.08)

85
    IMAX = find_param("RLL2SRV_IMAX", 3000)

86
    TCONST = find_param("RLL2SRV_TCONST", 0.5)

87


88
    kp_ff = max((P - I * TCONST) * TCONST  - D, 0)

89
    if found_line:

90
        set_param("RLL_RATE_FF", FF + kp_ff, "RLL2SRV_FF")

91
        set_param("RLL_RATE_P", D, "RLL2SRV_P")

92
        set_param("RLL_RATE_I", I * TCONST, "RLL2SRV_I")

93
        set_param("RLL_RATE_D", 0, "RLL2SRV_D")

94
        set_param("RLL_RATE_IMAX", IMAX/4500.0, "RLL2SRV_IMAX")

95


96
    if not changed_param:

97
        print("No fixed wing PID params")

98
        return

99


100
    print("Writing")

101
    tfile = fname + ".tmp"

102
    f = open(tfile,"w")

103
    for i in range(len(lines)):

104
        f.write("%s\n" % lines[i])

105
    f.close()

106
    os.rename(tfile, fname)

107


108
import argparse

109
parser = argparse.ArgumentParser(description='convert plane PIDs from old to new system')

110


111
parser.add_argument('param_file', nargs='+')

112
args = parser.parse_args()

113


114
for f in args.param_file:

115
    process_file(f)

116


117