2022-07-18 11:09:04 -03:00
|
|
|
import collections
|
|
|
|
|
2014-01-27 02:38:57 -04:00
|
|
|
import DataflashLog
|
2022-07-18 11:09:04 -03:00
|
|
|
from LogAnalyzer import Test, TestResult
|
2017-09-29 02:21:28 -03:00
|
|
|
from VehicleType import VehicleType
|
2014-01-27 02:38:57 -04:00
|
|
|
|
|
|
|
|
|
|
|
class TestPitchRollCoupling(Test):
|
2014-08-12 12:54:15 -03:00
|
|
|
'''test for divergence between input and output pitch/roll, i.e. mechanical failure or bad PID tuning'''
|
2022-07-18 11:09:04 -03:00
|
|
|
|
2014-08-12 12:54:15 -03:00
|
|
|
# TODO: currently we're only checking for roll/pitch outside of max lean angle, will come back later to analyze roll/pitch in versus out values
|
|
|
|
|
|
|
|
def __init__(self):
|
|
|
|
Test.__init__(self)
|
|
|
|
self.name = "Pitch/Roll"
|
2022-07-18 11:09:04 -03:00
|
|
|
self.enable = True # TEMP
|
2014-08-12 12:54:15 -03:00
|
|
|
|
|
|
|
def run(self, logdata, verbose):
|
|
|
|
self.result = TestResult()
|
|
|
|
self.result.status = TestResult.StatusType.GOOD
|
|
|
|
|
2017-09-23 22:51:51 -03:00
|
|
|
if logdata.vehicleType != VehicleType.Copter:
|
2014-08-12 12:54:15 -03:00
|
|
|
self.result.status = TestResult.StatusType.NA
|
|
|
|
return
|
|
|
|
|
|
|
|
if not "ATT" in logdata.channels:
|
|
|
|
self.result.status = TestResult.StatusType.UNKNOWN
|
|
|
|
self.result.statusMessage = "No ATT log data"
|
|
|
|
return
|
|
|
|
|
2017-12-01 18:11:11 -04:00
|
|
|
if not "CTUN" in logdata.channels:
|
|
|
|
self.result.status = TestResult.StatusType.UNKNOWN
|
|
|
|
self.result.statusMessage = "No CTUN log data"
|
|
|
|
return
|
|
|
|
|
|
|
|
if "BarAlt" in logdata.channels['CTUN']:
|
|
|
|
self.ctun_baralt_att = 'BarAlt'
|
|
|
|
else:
|
|
|
|
self.ctun_baralt_att = 'BAlt'
|
|
|
|
|
2014-08-12 12:54:15 -03:00
|
|
|
# figure out where each mode begins and ends, so we can treat auto and manual modes differently and ignore acro/tune modes
|
2022-07-18 11:09:04 -03:00
|
|
|
autoModes = [
|
|
|
|
"RTL",
|
|
|
|
"AUTO",
|
|
|
|
"LAND",
|
|
|
|
"LOITER",
|
|
|
|
"GUIDED",
|
|
|
|
"CIRCLE",
|
|
|
|
"OF_LOITER",
|
|
|
|
"POSHOLD",
|
|
|
|
"BRAKE",
|
|
|
|
"AVOID_ADSB",
|
|
|
|
"GUIDED_NOGPS",
|
|
|
|
"SMARTRTL",
|
|
|
|
]
|
2017-09-29 20:17:26 -03:00
|
|
|
# use CTUN RollIn/DesRoll + PitchIn/DesPitch
|
|
|
|
manualModes = ["STABILIZE", "DRIFT", "ALTHOLD", "ALT_HOLD", "POSHOLD"]
|
|
|
|
# ignore data from these modes:
|
2022-07-18 11:09:04 -03:00
|
|
|
ignoreModes = [
|
|
|
|
"ACRO",
|
|
|
|
"SPORT",
|
|
|
|
"FLIP",
|
|
|
|
"AUTOTUNE",
|
|
|
|
"",
|
|
|
|
"THROW",
|
|
|
|
]
|
|
|
|
autoSegments = [] # list of (startLine,endLine) pairs
|
2014-08-12 12:54:15 -03:00
|
|
|
manualSegments = [] # list of (startLine,endLine) pairs
|
|
|
|
orderedModes = collections.OrderedDict(sorted(logdata.modeChanges.items(), key=lambda t: t[0]))
|
2022-07-18 11:09:04 -03:00
|
|
|
isAuto = False # we always start in a manual control mode
|
2014-08-12 12:54:15 -03:00
|
|
|
prevLine = 0
|
|
|
|
mode = ""
|
2022-07-18 11:09:04 -03:00
|
|
|
for line, modepair in orderedModes.items():
|
2014-08-12 12:54:15 -03:00
|
|
|
mode = modepair[0].upper()
|
|
|
|
if prevLine == 0:
|
|
|
|
prevLine = line
|
|
|
|
if mode in autoModes:
|
|
|
|
if not isAuto:
|
2022-07-18 11:09:04 -03:00
|
|
|
manualSegments.append((prevLine, line - 1))
|
2014-08-12 12:54:15 -03:00
|
|
|
prevLine = line
|
|
|
|
isAuto = True
|
|
|
|
elif mode in manualModes:
|
|
|
|
if isAuto:
|
2022-07-18 11:09:04 -03:00
|
|
|
autoSegments.append((prevLine, line - 1))
|
2014-08-12 12:54:15 -03:00
|
|
|
prevLine = line
|
|
|
|
isAuto = False
|
|
|
|
elif mode in ignoreModes:
|
|
|
|
if isAuto:
|
2022-07-18 11:09:04 -03:00
|
|
|
autoSegments.append((prevLine, line - 1))
|
2014-08-12 12:54:15 -03:00
|
|
|
else:
|
2022-07-18 11:09:04 -03:00
|
|
|
manualSegments.append((prevLine, line - 1))
|
2014-08-12 12:54:15 -03:00
|
|
|
prevLine = 0
|
|
|
|
else:
|
|
|
|
raise Exception("Unknown mode in TestPitchRollCoupling: %s" % mode)
|
|
|
|
# and handle the last segment, which doesn't have an ending
|
|
|
|
if mode in autoModes:
|
2022-07-18 11:09:04 -03:00
|
|
|
autoSegments.append((prevLine, logdata.lineCount))
|
2014-08-12 12:54:15 -03:00
|
|
|
elif mode in manualModes:
|
2022-07-18 11:09:04 -03:00
|
|
|
manualSegments.append((prevLine, logdata.lineCount))
|
2014-08-12 12:54:15 -03:00
|
|
|
|
|
|
|
# figure out max lean angle, the ANGLE_MAX param was added in AC3.1
|
|
|
|
maxLeanAngle = 45.0
|
|
|
|
if "ANGLE_MAX" in logdata.parameters:
|
|
|
|
maxLeanAngle = logdata.parameters["ANGLE_MAX"] / 100.0
|
2022-07-18 11:09:04 -03:00
|
|
|
maxLeanAngleBuffer = 10 # allow a buffer margin
|
2014-08-12 12:54:15 -03:00
|
|
|
|
|
|
|
# ignore anything below this altitude, to discard any data while not flying
|
|
|
|
minAltThreshold = 2.0
|
|
|
|
|
|
|
|
# look through manual+auto flight segments
|
|
|
|
# TODO: filter to ignore single points outside range?
|
2022-07-18 11:09:04 -03:00
|
|
|
(maxRoll, maxRollLine) = (0.0, 0)
|
2014-08-12 12:54:15 -03:00
|
|
|
(maxPitch, maxPitchLine) = (0.0, 0)
|
2022-07-18 11:09:04 -03:00
|
|
|
for (startLine, endLine) in manualSegments + autoSegments:
|
2014-08-12 12:54:15 -03:00
|
|
|
# quick up-front test, only fallover into more complex line-by-line check if max()>threshold
|
2022-07-18 11:09:04 -03:00
|
|
|
rollSeg = logdata.channels["ATT"]["Roll"].getSegment(startLine, endLine)
|
|
|
|
pitchSeg = logdata.channels["ATT"]["Pitch"].getSegment(startLine, endLine)
|
2014-08-12 12:54:15 -03:00
|
|
|
if not rollSeg.dictData and not pitchSeg.dictData:
|
|
|
|
continue
|
|
|
|
# check max roll+pitch for any time where relative altitude is above minAltThreshold
|
2022-07-18 11:09:04 -03:00
|
|
|
roll = max(abs(rollSeg.min()), abs(rollSeg.max()))
|
2014-08-12 12:54:15 -03:00
|
|
|
pitch = max(abs(pitchSeg.min()), abs(pitchSeg.max()))
|
2022-07-18 11:09:04 -03:00
|
|
|
if (roll > (maxLeanAngle + maxLeanAngleBuffer) and abs(roll) > abs(maxRoll)) or (
|
|
|
|
pitch > (maxLeanAngle + maxLeanAngleBuffer) and abs(pitch) > abs(maxPitch)
|
|
|
|
):
|
2014-08-12 12:54:15 -03:00
|
|
|
lit = DataflashLog.LogIterator(logdata, startLine)
|
2022-07-18 11:09:04 -03:00
|
|
|
assert lit.currentLine == startLine
|
2014-08-12 12:54:15 -03:00
|
|
|
while lit.currentLine <= endLine:
|
2017-12-01 18:11:11 -04:00
|
|
|
relativeAlt = lit["CTUN"][self.ctun_baralt_att]
|
2014-08-12 12:54:15 -03:00
|
|
|
if relativeAlt > minAltThreshold:
|
2022-07-18 11:09:04 -03:00
|
|
|
roll = lit["ATT"]["Roll"]
|
2014-08-12 12:54:15 -03:00
|
|
|
pitch = lit["ATT"]["Pitch"]
|
2022-07-18 11:09:04 -03:00
|
|
|
if abs(roll) > (maxLeanAngle + maxLeanAngleBuffer) and abs(roll) > abs(maxRoll):
|
2014-08-12 12:54:15 -03:00
|
|
|
maxRoll = roll
|
|
|
|
maxRollLine = lit.currentLine
|
2022-07-18 11:09:04 -03:00
|
|
|
if abs(pitch) > (maxLeanAngle + maxLeanAngleBuffer) and abs(pitch) > abs(maxPitch):
|
2014-08-12 12:54:15 -03:00
|
|
|
maxPitch = pitch
|
|
|
|
maxPitchLine = lit.currentLine
|
2017-09-23 05:19:59 -03:00
|
|
|
next(lit)
|
2014-08-12 12:54:15 -03:00
|
|
|
# check for breaking max lean angles
|
2022-07-18 11:09:04 -03:00
|
|
|
if maxRoll and abs(maxRoll) > abs(maxPitch):
|
2014-08-12 12:54:15 -03:00
|
|
|
self.result.status = TestResult.StatusType.FAIL
|
2022-07-18 11:09:04 -03:00
|
|
|
self.result.statusMessage = "Roll (%.2f, line %d) > maximum lean angle (%.2f)" % (
|
|
|
|
maxRoll,
|
|
|
|
maxRollLine,
|
|
|
|
maxLeanAngle,
|
|
|
|
)
|
2014-08-12 12:54:15 -03:00
|
|
|
return
|
|
|
|
if maxPitch:
|
|
|
|
self.result.status = TestResult.StatusType.FAIL
|
2022-07-18 11:09:04 -03:00
|
|
|
self.result.statusMessage = "Pitch (%.2f, line %d) > maximum lean angle (%.2f)" % (
|
|
|
|
maxPitch,
|
|
|
|
maxPitchLine,
|
|
|
|
maxLeanAngle,
|
|
|
|
)
|
2014-08-12 12:54:15 -03:00
|
|
|
return
|
|
|
|
|
|
|
|
# TODO: use numpy/scipy to check Roll+RollIn curves for fitness (ignore where we're not airborne)
|
|
|
|
# ...
|