@ -12,6 +12,8 @@ import bisect
@@ -12,6 +12,8 @@ import bisect
import sys
import ctypes
from VehicleType import VehicleType , VehicleTypeString
class Format ( object ) :
''' Data channel format as specified by the FMT lines in the log file '''
def __init__ ( self , msgType , msgLen , name , types , labels ) :
@ -383,7 +385,7 @@ class DataflashLogHelper:
@@ -383,7 +385,7 @@ class DataflashLogHelper:
''' returns an human readable error string if the log is essentially empty, otherwise returns None '''
# naive check for now, see if the throttle output was ever above 20%
throttleThreshold = 20
if logdata . vehicleType == " ArduCopter " :
if logdata . vehicleType == VehicleType . Copter :
throttleThreshold = 200 # copter uses 0-1000, plane+rover use 0-100
if " CTUN " in logdata . channels :
maxThrottle = logdata . channels [ " CTUN " ] [ " ThrOut " ] . max ( )
@ -403,7 +405,8 @@ class DataflashLog(object):
@@ -403,7 +405,8 @@ class DataflashLog(object):
def __init__ ( self , logfile = None , format = " auto " , ignoreBadlines = False ) :
self . filename = None
self . vehicleType = " " # ArduCopter, ArduPlane, ArduRover, etc, verbatim as given by header
self . vehicleType = None # from VehicleType enumeration; value derived from header
self . vehicleTypeString = None # set at same time has the enum value
self . firmwareVersion = " "
self . firmwareHash = " "
self . freeRAM = 0
@ -425,7 +428,7 @@ class DataflashLog(object):
@@ -425,7 +428,7 @@ class DataflashLog(object):
def getCopterType ( self ) :
''' returns quad/hex/octo/tradheli if this is a copter log '''
if self . vehicleType != " ArduCopter " :
if self . vehicleType != VehicleType . Copter :
return None
motLabels = [ ]
if " MOT " in self . formats : # not listed in PX4 log header for some reason?
@ -492,6 +495,22 @@ class DataflashLog(object):
@@ -492,6 +495,22 @@ class DataflashLog(object):
# TODO: calculate logging rate based on timestamps
# ...
msg_vehicle_to_vehicle_map = {
" ArduCopter " : VehicleType . Copter ,
" APM:Copter " : VehicleType . Copter ,
" ArduPlane " : VehicleType . Plane ,
" ArduRover " : VehicleType . Rover
}
# takes the vehicle type supplied via "MSG" and sets vehicleType from
# the VehicleType enumeration
def set_vehicleType_from_MSG_vehicle ( self , MSG_vehicle ) :
ret = self . msg_vehicle_to_vehicle_map . get ( MSG_vehicle , None )
if ret is None :
raise ValueError ( " Unknown vehicle type ( %s ) " % ( MSG_vehicle ) )
self . vehicleType = ret
self . vehicleTypeString = VehicleTypeString [ ret ]
def process ( self , lineNumber , e ) :
if e . NAME == ' FMT ' :
cls = e . to_class ( )
@ -505,15 +524,14 @@ class DataflashLog(object):
@@ -505,15 +524,14 @@ class DataflashLog(object):
elif e . NAME == " MSG " :
if not self . vehicleType :
tokens = e . Message . split ( ' ' )
vehicleTypes = [ " ArduPlane " , " ArduCopter " , " ArduRover " ]
self . vehicleType = tokens [ 0 ]
self . set_vehicleType_from_MSG_vehicle ( tokens [ 0 ] ) ;
self . firmwareVersion = tokens [ 1 ]
if len ( tokens ) == 3 :
self . firmwareHash = tokens [ 2 ] [ 1 : - 1 ]
else :
self . messages [ lineNumber ] = e . Message
elif e . NAME == " MODE " :
if self . vehicleType in [ " ArduCopter " ] :
if self . vehicleType == VehicleType . Copter :
try :
modes = { 0 : ' STABILIZE ' ,
1 : ' ACRO ' ,
@ -530,13 +548,21 @@ class DataflashLog(object):
@@ -530,13 +548,21 @@ class DataflashLog(object):
14 : ' FLIP ' ,
15 : ' AUTOTUNE ' ,
16 : ' HYBRID ' , }
self . modeChanges [ lineNumber ] = ( modes [ int ( e . Mode ) ] , e . ThrCrs )
if hasattr ( e , ' ThrCrs ' ) :
self . modeChanges [ lineNumber ] = ( modes [ int ( e . Mode ) ] , e . ThrCrs )
else :
# assume it has ModeNum:
self . modeChanges [ lineNumber ] = ( modes [ int ( e . Mode ) ] , e . ModeNum )
except :
self . modeChanges [ lineNumber ] = ( e . Mode , e . ThrCrs )
elif self . vehicleType in [ " ArduPlane " , " APM:Plane " , " ArduRover " , " APM:Rover " , " APM:Copter " ] :
if hasattr ( e , ' ThrCrs ' ) :
self . modeChanges [ lineNumber ] = ( e . Mode , e . ThrCrs )
else :
# assume it has ModeNum:
self . modeChanges [ lineNumber ] = ( e . Mode , e . ModeNum )
elif self . vehicleType in [ VehicleType . Plane , VehicleType . Copter , VehicleType . Rover ] :
self . modeChanges [ lineNumber ] = ( e . Mode , e . ModeNum )
else :
raise Exception ( " Unknown log type for MODE line vehicletype=( {} ) linw=( {} ) " . format ( self . vehicleType , repr ( e ) ) )
raise Exception ( " Unknown log type for MODE line vehicletype=( {} ) linw=( {} ) " . format ( self . vehicleTypeString , repr ( e ) ) )
# anything else must be the log data
else :
groupName = e . NAME
@ -583,7 +609,7 @@ class DataflashLog(object):
@@ -583,7 +609,7 @@ class DataflashLog(object):
elif tokens2 [ 0 ] in knownHardwareTypes :
self . hardwareType = line # not sure if we can parse this more usefully, for now only need to report it back verbatim
elif ( len ( tokens2 ) == 2 or len ( tokens2 ) == 3 ) and tokens2 [ 1 ] [ 0 ] . lower ( ) == " v " : # e.g. ArduCopter V3.1 (5c6503e2)
self . vehicleType = tokens2 [ 0 ]
self . set_vehicleType_from_MSG_vehicle ( tokens2 [ 0 ] )
self . firmwareVersion = tokens2 [ 1 ]
if len ( tokens2 ) == 3 :
self . firmwareHash = tokens2 [ 2 ] [ 1 : - 1 ]