Book a Demo!
CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutPoliciesSign UpSign In
Ardupilot
GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Tools/scripts/convert_plane_PID.py
9422 views
1
#!/usr/bin/env python3

2


3
# flake8: noqa

4


5
'''

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

7
'''

8


9
import os

10
import re

11


12
found_line = False

13
changed_param = False

14


15
def process_file(fname):

16
    global found_line

17
    global changed_param

18


19
    found_line = False

20
    changed_param = False

21


22
    print("Processing %s" % fname)

23
    lines = open(fname).readlines()

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

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

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

27
        lines = lines[:-1]

28


29
    def find_line(pname):

30
        global found_line

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

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

33
                found_line = True

34
                return i

35
        return None

36


37
    def find_param(pname, dvalue):

38
        i = find_line(pname)

39
        if i is None:

40
            return dvalue

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

42
        a = s.split()

43
        return float(a[1])

44


45
    def set_param(pname, value, old_name):

46
        global changed_param

47
        i = find_line(pname)

48
        if i is None:

49
            i = find_line(old_name)

50
        if i is None:

51
            changed_param = True

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

53
            return

54
        # maintain separator if possible

55
        sep = " "

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

57
        if m is not None:

58
            sep = m.group(1)

59
        changed_param = True

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

61


62
    print("Converting pitch")

63
    FF = find_param("PTCH2SRV_FF", 0)

64
    P = find_param("PTCH2SRV_P", 1.0)

65
    I = find_param("PTCH2SRV_I", 0.3)

66
    D = find_param("PTCH2SRV_D", 0.04)

67
    IMAX = find_param("PTCH2SRV_IMAX", 3000)

68
    TCONST = find_param("PTCH2SRV_TCONST", 0.5)

69


70
    if FF <= 0:

71
        I = max(I,0.3)

72


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

74
    if found_line:

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

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

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

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

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

80


81
    found_line = False

82


83
    print("Converting roll")

84
    FF = find_param("RLL2SRV_FF", 0)

85
    P = find_param("RLL2SRV_P", 1.0)

86
    I = find_param("RLL2SRV_I", 0.3)

87
    D = find_param("RLL2SRV_D", 0.08)

88
    IMAX = find_param("RLL2SRV_IMAX", 3000)

89
    TCONST = find_param("RLL2SRV_TCONST", 0.5)

90


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

92
    if found_line:

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

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

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

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

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

98


99
    if not changed_param:

100
        print("No fixed wing PID params")

101
        return

102


103
    print("Writing")

104
    tfile = fname + ".tmp"

105
    f = open(tfile,"w")

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

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

108
    f.close()

109
    os.rename(tfile, fname)

110


111
import argparse

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

113


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

115
args = parser.parse_args()

116


117
for f in args.param_file:

118
    process_file(f)

119


120