2014-01-27 02:38:57 -04:00
from LogAnalyzer import Test , TestResult
import DataflashLog
2014-03-03 13:46:17 -04:00
import math
2014-01-27 02:38:57 -04:00
class TestCompass ( Test ) :
2014-08-12 12:54:15 -03:00
''' test for compass offsets and throttle interference '''
def __init__ ( self ) :
Test . __init__ ( self )
self . name = " Compass "
def run ( self , logdata , verbose ) :
self . result = TestResult ( )
self . result . status = TestResult . StatusType . GOOD
2014-10-25 18:30:52 -03:00
def vec_len ( x ) :
2015-10-13 20:13:50 -03:00
return math . sqrt ( x [ 0 ] * * 2 + x [ 1 ] * * 2 + x [ 2 ] * * 2 )
2014-10-25 18:30:52 -03:00
def FAIL ( ) :
self . result . status = TestResult . StatusType . FAIL
def WARN ( ) :
if self . result . status != TestResult . StatusType . FAIL :
self . result . status = TestResult . StatusType . WARN
2014-08-12 12:58:02 -03:00
try :
2014-10-25 18:30:52 -03:00
warnOffset = 300
failOffset = 500
param_offsets = (
logdata . parameters [ " COMPASS_OFS_X " ] ,
logdata . parameters [ " COMPASS_OFS_Y " ] ,
logdata . parameters [ " COMPASS_OFS_Z " ]
)
if vec_len ( param_offsets ) > failOffset :
FAIL ( )
self . result . statusMessage = " FAIL: Large compass offset params (X: %.2f , Y: %.2f , Z: %.2f ) \n " % ( param_offsets [ 0 ] , param_offsets [ 1 ] , param_offsets [ 2 ] )
elif vec_len ( param_offsets ) > warnOffset :
WARN ( )
self . result . statusMessage = " WARN: Large compass offset params (X: %.2f , Y: %.2f , Z: %.2f ) \n " % ( param_offsets [ 0 ] , param_offsets [ 1 ] , param_offsets [ 2 ] )
2014-08-12 12:58:02 -03:00
if " MAG " in logdata . channels :
2014-10-25 18:30:52 -03:00
max_log_offsets = zip (
map ( lambda x : x [ 1 ] , logdata . channels [ " MAG " ] [ " OfsX " ] . listData ) ,
map ( lambda x : x [ 1 ] , logdata . channels [ " MAG " ] [ " OfsY " ] . listData ) ,
map ( lambda x : x [ 1 ] , logdata . channels [ " MAG " ] [ " OfsZ " ] . listData )
)
max_log_offsets = reduce ( lambda x , y : x if vec_len ( x ) > vec_len ( y ) else y , max_log_offsets )
if vec_len ( max_log_offsets ) > failOffset :
FAIL ( )
self . result . statusMessage + = " FAIL: Large compass offset in MAG data (X: %.2f , Y: %.2f , Z: %.2f ) \n " % ( max_log_offsets [ 0 ] , max_log_offsets [ 1 ] , max_log_offsets [ 2 ] )
elif vec_len ( max_log_offsets ) > warnOffset :
WARN ( )
self . result . statusMessage + = " WARN: Large compass offset in MAG data (X: %.2f , Y: %.2f , Z: %.2f ) \n " % ( max_log_offsets [ 0 ] , max_log_offsets [ 1 ] , max_log_offsets [ 2 ] )
2014-08-12 12:58:02 -03:00
# check for mag field length change, and length outside of recommended range
if " MAG " in logdata . channels :
percentDiffThresholdWARN = 0.25
percentDiffThresholdFAIL = 0.35
minMagFieldThreshold = 120.0
maxMagFieldThreshold = 550.0
index = 0
length = len ( logdata . channels [ " MAG " ] [ " MagX " ] . listData )
magField = [ ]
( minMagField , maxMagField ) = ( None , None )
( minMagFieldLine , maxMagFieldLine ) = ( None , None )
zerosFound = False
while index < length :
mx = logdata . channels [ " MAG " ] [ " MagX " ] . listData [ index ] [ 1 ]
my = logdata . channels [ " MAG " ] [ " MagY " ] . listData [ index ] [ 1 ]
mz = logdata . channels [ " MAG " ] [ " MagZ " ] . listData [ index ] [ 1 ]
if ( ( mx == 0 ) and ( my == 0 ) and ( mz == 0 ) ) : # sometimes they're zero, not sure why, same reason as why we get NaNs as offsets?
zerosFound = True
else :
mf = math . sqrt ( mx * mx + my * my + mz * mz )
magField . append ( mf )
if mf < minMagField :
minMagField = mf
minMagFieldLine = logdata . channels [ " MAG " ] [ " MagX " ] . listData [ index ] [ 0 ]
if mf > maxMagField :
maxMagField = mf
maxMagFieldLine = logdata . channels [ " MAG " ] [ " MagX " ] . listData [ index ] [ 0 ]
if index == 0 :
( minMagField , maxMagField ) = ( mf , mf )
index + = 1
percentDiff = ( maxMagField - minMagField ) / minMagField
if percentDiff > percentDiffThresholdFAIL :
2014-10-25 18:30:52 -03:00
FAIL ( )
2014-08-12 12:58:02 -03:00
self . result . statusMessage = self . result . statusMessage + " Large change in mag_field ( %.2f %% ) \n " % ( percentDiff * 100 )
elif percentDiff > percentDiffThresholdWARN :
2014-10-25 18:30:52 -03:00
WARN ( )
2014-08-12 12:58:02 -03:00
self . result . statusMessage = self . result . statusMessage + " Moderate change in mag_field ( %.2f %% ) \n " % ( percentDiff * 100 )
2014-08-12 12:54:15 -03:00
else :
2014-08-12 12:58:02 -03:00
self . result . statusMessage = self . result . statusMessage + " mag_field interference within limits ( %.2f %% ) \n " % ( percentDiff * 100 )
if minMagField < minMagFieldThreshold :
self . result . statusMessage = self . result . statusMessage + " Min mag field length ( %.2f ) < recommended ( %.2f ) \n " % ( minMagField , minMagFieldThreshold )
if maxMagField > maxMagFieldThreshold :
self . result . statusMessage = self . result . statusMessage + " Max mag field length ( %.2f ) > recommended ( %.2f ) \n " % ( maxMagField , maxMagFieldThreshold )
if zerosFound :
2014-10-25 18:30:52 -03:00
WARN ( )
2014-08-12 12:58:02 -03:00
self . result . statusMessage = self . result . statusMessage + " All zeros found in MAG X/Y/Z log data \n "
if verbose :
self . result . statusMessage = self . result . statusMessage + " Min mag_field of %.2f on line %d \n " % ( minMagField , minMagFieldLine )
self . result . statusMessage = self . result . statusMessage + " Max mag_field of %.2f on line %d \n " % ( maxMagField , maxMagFieldLine )
2014-08-12 12:54:15 -03:00
else :
2014-08-12 12:58:02 -03:00
self . result . statusMessage = self . result . statusMessage + " No MAG data, unable to test mag_field interference \n "
except KeyError as e :
self . result . status = TestResult . StatusType . FAIL
self . result . statusMessage = str ( e ) + ' not found '
2014-03-03 13:46:17 -04:00
2014-01-27 02:38:57 -04:00