ardupilot/Tools/LogAnalyzer/tests/TestGPSGlitch.py

from LogAnalyzer import Test,TestResult
import DataflashLog


class TestGPSGlitch(Test):
	'''test for GPS glitch reporting or bad GPS data (satellite count, hdop)'''

	def __init__(self):
		Test.__init__(self)
		self.name = "GPS"

        def findSatsChan(self, channels):
                for chan in "NSats","NSat","numSV":
                        if chan in channels:
                                return channels[chan]

        def findHDopChan(self, channels):
                for chan in "HDop","HDp","EPH":
                        if chan in channels:
                                return channels[chan]

	def run(self, logdata, verbose):
		self.result = TestResult()
		self.result.status = TestResult.StatusType.GOOD

		if "GPS" not in logdata.channels:
			self.result.status = TestResult.StatusType.UNKNOWN
			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
		maxHDopFAIL = 10.0
		satsChan = self.findSatsChan(logdata.channels["GPS"])
		hdopChan = self.findHDopChan(logdata.channels["GPS"])
		foundBadSatsWarn = satsChan.min() < minSatsWARN
		foundBadHDopWarn = hdopChan.max()  > maxHDopWARN
		foundBadSatsFail = satsChan.min() < minSatsFAIL
		foundBadHDopFail = hdopChan.max()  > maxHDopFAIL
		satsMsg = "Min satellites: %s, Max HDop: %s" % (satsChan.min(), hdopChan.max())
		if gpsGlitchCount:
			self.result.statusMessage = self.result.statusMessage + "\n" + satsMsg
		if foundBadSatsFail or foundBadHDopFail:
			if not gpsGlitchCount:
				self.result.status = TestResult.StatusType.FAIL
				self.result.statusMessage = satsMsg
		elif foundBadSatsWarn or foundBadHDopWarn:
			if not gpsGlitchCount:
				self.result.status = TestResult.StatusType.WARN
				self.result.statusMessage = satsMsg