From 2a406ac699518750016554fbd97d2b2b07af71ab Mon Sep 17 00:00:00 2001 From: Andrew Chapman Date: Wed, 26 Feb 2014 13:50:55 +0100 Subject: [PATCH] LogAnalyzer: added GPS glitch detection to GPS test --- Tools/LogAnalyzer/DataflashLog.py | 12 +++- Tools/LogAnalyzer/LogAnalyzer.py | 2 +- Tools/LogAnalyzer/tests/TestBalanceTwist.py | 3 +- Tools/LogAnalyzer/tests/TestGPSGlitch.py | 32 +++++++--- Tools/LogAnalyzer/tests/TestParams.py | 3 + Tools/LogAnalyzer/tests/TestPerformance.py | 1 + .../tests/TestPitchRollCoupling.py | 61 +++++++++++++------ Tools/LogAnalyzer/tests/TestVibration.py | 4 ++ 8 files changed, 89 insertions(+), 29 deletions(-) diff --git a/Tools/LogAnalyzer/DataflashLog.py b/Tools/LogAnalyzer/DataflashLog.py index af525e76f5..f2099dbdb3 100644 --- a/Tools/LogAnalyzer/DataflashLog.py +++ b/Tools/LogAnalyzer/DataflashLog.py @@ -8,6 +8,7 @@ import pprint # temp import collections import os import numpy +import bisect class Format: @@ -50,6 +51,7 @@ class Channel: def avg(self): return numpy.mean(self.dictData.values()) def getNearestValue(self, lineNumber, lookForwards=True): + # TODO: redo Channel.getNearestValue() using listData and bisect, profile speed of TestUnderpowered before/after '''find the nearest data value to the given lineNumber, defaults to looking forwards. Returns (value,lineNumber)''' if lineNumber in self.dictData: return (self.dictData[lineNumber], lineNumber) @@ -72,6 +74,11 @@ class Channel: return prevValue weight = (lineNumber-prevValueLine) / float(nextValueLine-prevValueLine) return ((weight*prevValue) + ((1-weight)*nextValue)) + def getIndexOf(self, lineNumber): + index = bisect.bisect_left(self.listData, (lineNumber,0)) - 1 + print "INDEX: %d" % index + assert(self.listData[index][0] == lineNumber) + return index # class LogIterator: # '''Smart iterator that can move through a log by line number and maintain an index into the nearest values of all data channels''' @@ -217,8 +224,8 @@ class DataflashLog: def read(self, logfile, ignoreBadlines=False): '''returns True on successful log read (including bad lines if ignoreBadlines==True), False otherwise''' # TODO: dataflash log parsing code is *SUPER* hacky, should re-write more methodically - # TODO: identify and bail on pre-3.0 logs, I think they're not worth supporting at this point - + # TODO: fix error handling of logfile reading, return code vs exception, error message reporting + # TODO: report number of lines skipped during read self.filename = logfile f = open(self.filename, 'r') lineNumber = 0 @@ -311,6 +318,7 @@ class DataflashLog: # gather some general stats about the log self.lineCount = lineNumber self.filesizeKB = os.path.getsize(self.filename) / 1024.0 + # TODO: switch duration calculation to use TimeMS values rather than GPS timestemp if "GPS" in self.channels: # the GPS time label changed at some point, need to handle both timeLabel = "TimeMS" diff --git a/Tools/LogAnalyzer/LogAnalyzer.py b/Tools/LogAnalyzer/LogAnalyzer.py index d77cc1614d..e52e059bd3 100755 --- a/Tools/LogAnalyzer/LogAnalyzer.py +++ b/Tools/LogAnalyzer/LogAnalyzer.py @@ -118,7 +118,7 @@ class TestSuite: print " %20s: UNKNOWN %-50s%s" % (test.name, test.result.statusMessage,execTime) if test.result.extraFeedback: for line in test.result.extraFeedback.strip().split('\n'): - print " %20s %s" % ("",line) + print " %29s %s" % ("",line) print '\n' print 'The Log Analyzer is currently BETA code.\nFor any support or feedback on the log analyzer please email Andrew Chapman (amchapman@gmail.com)' diff --git a/Tools/LogAnalyzer/tests/TestBalanceTwist.py b/Tools/LogAnalyzer/tests/TestBalanceTwist.py index ee20de6c11..bd25e95d1b 100644 --- a/Tools/LogAnalyzer/tests/TestBalanceTwist.py +++ b/Tools/LogAnalyzer/tests/TestBalanceTwist.py @@ -7,12 +7,13 @@ class TestBalanceTwist(Test): def __init__(self): self.name = "Balance/Twist" + self.enable = False # TEMP def run(self, logdata): self.result = TestResult() self.result.status = TestResult.StatusType.PASS - if logdata.vehicleType == "ArduPlane": + if logdata.vehicleType != "ArduCopter": self.result.status = TestResult.StatusType.NA # TODO: implement copter test for unbalanced or twisted copter frame \ No newline at end of file diff --git a/Tools/LogAnalyzer/tests/TestGPSGlitch.py b/Tools/LogAnalyzer/tests/TestGPSGlitch.py index 2762749b5a..7c3a745a07 100644 --- a/Tools/LogAnalyzer/tests/TestGPSGlitch.py +++ b/Tools/LogAnalyzer/tests/TestGPSGlitch.py @@ -3,7 +3,7 @@ import DataflashLog class TestGPSGlitch(Test): - '''test for bad GPS data (satellite count, hdop), and later for sudden repositioning beyond what the drone could do''' + '''test for GPS glitch reporting or bad GPS data (satellite count, hdop)''' def __init__(self): self.name = "GPS" @@ -17,7 +17,21 @@ class TestGPSGlitch(Test): self.result.statusMessage = "No GPS log data" return + # glitch protection is currently copter-only, but might be added to other vehicle types later and there's no harm in leaving the test in for all + gpsGlitchCount = 0 + if "ERR" in logdata.channels: + assert(len(logdata.channels["ERR"]["Subsys"].listData) == len(logdata.channels["ERR"]["ECode"].listData)) + for i in range(len(logdata.channels["ERR"]["Subsys"].listData)): + subSys = logdata.channels["ERR"]["Subsys"].listData[i][1] + eCode = logdata.channels["ERR"]["ECode"].listData[i][1] + if subSys == 11 and (eCode == 2): + gpsGlitchCount += 1 + if gpsGlitchCount: + self.result.status = TestResult.StatusType.FAIL + self.result.statusMessage = "GPS glitch errors found (%d)" % gpsGlitchCount + # define and check different thresholds for WARN level and FAIL level + # TODO: for plane, only check after first instance of throttle > 0, or after takeoff if we can reliably detect it minSatsWARN = 6 minSatsFAIL = 5 maxHDopWARN = 3.0 @@ -26,12 +40,14 @@ class TestGPSGlitch(Test): foundBadHDopWarn = logdata.channels["GPS"]["HDop"].max() > maxHDopWARN foundBadSatsFail = logdata.channels["GPS"]["NSats"].min() < minSatsFAIL foundBadHDopFail = logdata.channels["GPS"]["HDop"].max() > maxHDopFAIL + satsMsg = "Min satellites: %s, Max HDop: %s" % (logdata.channels["GPS"]["NSats"].min(), logdata.channels["GPS"]["HDop"].max()) + if gpsGlitchCount: + self.result.extraFeedback = satsMsg if foundBadSatsFail or foundBadHDopFail: - self.result.status = TestResult.StatusType.FAIL - self.result.statusMessage = "Min satellites: %s, Max HDop: %s" % (logdata.channels["GPS"]["NSats"].min(), logdata.channels["GPS"]["HDop"].max()) + if not gpsGlitchCount: + self.result.status = TestResult.StatusType.FAIL + self.result.statusMessage = satsMsg elif foundBadSatsWarn or foundBadHDopWarn: - self.result.status = TestResult.StatusType.WARN - self.result.statusMessage = "Min satellites: %s, Max HDop: %s" % (logdata.channels["GPS"]["NSats"].min(), logdata.channels["GPS"]["HDop"].max()) - - # TODO: also test for sudden repositioning beyond what the drone could reasonably achieve, like is done with glitch protection - or is that logged when it happens? - # ... + if not gpsGlitchCount: + self.result.status = TestResult.StatusType.WARN + self.result.statusMessage = satsMsg diff --git a/Tools/LogAnalyzer/tests/TestParams.py b/Tools/LogAnalyzer/tests/TestParams.py index a25d623435..e2d13680d6 100644 --- a/Tools/LogAnalyzer/tests/TestParams.py +++ b/Tools/LogAnalyzer/tests/TestParams.py @@ -50,6 +50,9 @@ class TestParams(Test): elif logdata.vehicleType == "ArduPlane": # TODO: add parameter checks for plane... pass + elif logdata.vehicleType == "ArduRover": + # TODO: add parameter checks for rover... + pass if self.result.status == TestResult.StatusType.FAIL: self.result.statusMessage = "Bad parameters found:" diff --git a/Tools/LogAnalyzer/tests/TestPerformance.py b/Tools/LogAnalyzer/tests/TestPerformance.py index c84ed2a6ef..1042f87015 100644 --- a/Tools/LogAnalyzer/tests/TestPerformance.py +++ b/Tools/LogAnalyzer/tests/TestPerformance.py @@ -12,6 +12,7 @@ class TestPerformance(Test): self.result = TestResult() self.result.status = TestResult.StatusType.PASS + # this test should be valid for all vehicle types, just need to figure out why PM logging data is different in each if logdata.vehicleType != "ArduCopter": self.result.status = TestResult.StatusType.NA return diff --git a/Tools/LogAnalyzer/tests/TestPitchRollCoupling.py b/Tools/LogAnalyzer/tests/TestPitchRollCoupling.py index 70113b8aaf..7b911f5b19 100644 --- a/Tools/LogAnalyzer/tests/TestPitchRollCoupling.py +++ b/Tools/LogAnalyzer/tests/TestPitchRollCoupling.py @@ -13,6 +13,14 @@ class TestPitchRollCoupling(Test): self.result = TestResult() self.result.status = TestResult.StatusType.PASS + if logdata.vehicleType != "ArduCopter": + self.result.status = TestResult.StatusType.NA + + if not "ATT" in logdata.channels: + self.result.status = TestResult.StatusType.UNKNOWN + self.result.statusMessage = "No ATT log data" + return + # TODO: implement pitch/roll input/output divergence testing - # note: names changed from PitchIn to DesPitch at some point, check for both @@ -27,10 +35,9 @@ class TestPitchRollCoupling(Test): # figure out where each mode begins and ends, so we can treat auto and manual modes differently - # TODO: fill in all known modes here - autoModes = ["RTL","AUTO","LAND","LOITER"] # use NTUN DRol+DPit - manualModes = ["STABILIZE","DRIFT","ALT_HOLD"] # use CTUN RollIn/DesRoll + PitchIn/DesPitch - acroModes = ["ACRO","SPORT"] # ignore data from acro modes + autoModes = ["RTL","AUTO","LAND","LOITER","GUIDED","CIRCLE","OF_LOITER"] # use NTUN DRol+DPit + manualModes = ["STABILIZE","DRIFT","ALT_HOLD"] # use CTUN RollIn/DesRoll + PitchIn/DesPitch + ignoreModes = ["ACRO","SPORT","FLIP","AUTOTUNE"] # ignore data from these modes autoSegments = [] # list of (startLine,endLine) pairs manualSegments = [] # list of (startLine,endLine) pairs orderedModes = collections.OrderedDict(sorted(logdata.modeChanges.items(), key=lambda t: t[0])) @@ -39,25 +46,45 @@ class TestPitchRollCoupling(Test): for line,modepair in orderedModes.iteritems(): mode = modepair[0].upper() if mode in autoModes: - print "On line %d mode changed to %s (AUTO)" % (line,mode) + print "On line %d mode changed to %s (AUTO)" % (line,mode) # TEMP if not isAuto: manualSegments.append((prevLine,line-1)) - print " Previous manual segment: " + `(prevLine,line-1)` + print " Previous manual segment: " + `(prevLine,line-1)` # TEMP + prevLine = line isAuto = True elif mode in manualModes: - print "On line %d mode changed to %s (MANUAL)" % (line,mode) + print "On line %d mode changed to %s (MANUAL)" % (line,mode) # TEMP if isAuto: autoSegments.append((prevLine,line-1)) - print " Previous auto segment: " + `(prevLine,line-1)` + print " Previous auto segment: " + `(prevLine,line-1)` # TEMP + prevLine = line isAuto = False - elif mode in acroModes: + elif mode in ignoreModes: pass else: - raise Exception("Unknown mode: %s" % mode) - prevLine = line - if isAuto: - autoSegments.append((prevLine,logdata.lineCount)) - print " Previous auto segment: " + `(prevLine,logdata.lineCount)` - else: - manualSegments.append((prevLine,logdata.lineCount)) - print " Previous manual segment: " + `(prevLine,logdata.lineCount)` + raise Exception("Unknown mode in TestPitchRollCoupling: %s" % mode) + + # look through manual segments + for startLine,endLine in manualSegments: + (value,attLine) = logdata.channels["ATT"]["Roll"].getNearestValue(startLine, lookForwards=True) + print "Nearest ATT line after %d is %d" % (startLine,attLine) + index = logdata.channels["ATT"]["Roll"].getIndexOf(attLine) + print "First ATT line in manual segment (%d,%d) is line %d" % (startLine,endLine,logdata.channels["ATT"]["Roll"].listData[index][0]) + + + + + + + + # look through auto segments + # ... + + + + + + + + + diff --git a/Tools/LogAnalyzer/tests/TestVibration.py b/Tools/LogAnalyzer/tests/TestVibration.py index 7eadc51cf9..6100fc0cf2 100644 --- a/Tools/LogAnalyzer/tests/TestVibration.py +++ b/Tools/LogAnalyzer/tests/TestVibration.py @@ -14,6 +14,10 @@ class TestVibration(Test): def run(self, logdata): self.result = TestResult() + if logdata.vehicleType != "ArduCopter": + self.result.status = TestResult.StatusType.NA + return + # constants gravity = -9.81 aimRangeWarnXY = 1.5