kenozooid.driver.su

1 # 2 # Kenozooid - dive planning and analysis toolbox. 3 # 4 # Copyright (C) 2009-2019 by Artur Wroblewski <wrobell@riseup.net> 5 # 6 # This program is free software: you can redistribute it and/or modify 7 # it under the terms of the GNU General Public License as published by 8 # the Free Software Foundation, either version 3 of the License, or 9 # (at your option) any later version. 10 # 11 # This program is distributed in the hope that it will be useful, 12 # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 # GNU General Public License for more details. 15 # 16 # You should have received a copy of the GNU General Public License 17 # along with this program. If not, see <http://www.gnu.org/licenses/>. 18 # 19 20 """ 21 Driver for Reefnet Sensus Ultra dive logger. 22 23 It uses libdivecomputer library from 24 25 http://divesoftware.org/libdc/ 26 27 Tested with libdivecomputer version: 781f0db71f7a06591e819f7960dd5b504b4f16a0. 28 """ 29 30 import ctypes as ct 31 from datetime import datetime 32 from struct import unpack 33 from collections import namedtuple 34 from functools import partial 35 from queue import Queue, Full, Empty 36 from concurrent.futures import ThreadPoolExecutor 37 import time 38 import operator 39 40 import logging 41 log = logging.getLogger('kenozooid.driver.su') 42 43 import kenozooid.uddf as ku 44 import kenozooid.data as kd 45 import kenozooid.component as kc 46 from kenozooid.driver import DeviceDriver, DataParser, DeviceError 47 from kenozooid.units import C2K 48 49 SIZE_MEM_USER = 16384 50 SIZE_MEM_DATA = 2080768 51 SIZE_MEM_HANDSHAKE = 24 52 SIZE_MEM_SENSE = 6 53 54 START_HANDSHAKE = 0 55 END_HANDSHAKE = START_USER = SIZE_MEM_HANDSHAKE 56 END_USER = START_DATA = START_USER + SIZE_MEM_USER 57 END_DATA = START_DATA + SIZE_MEM_DATA 58 59 # Reefnet Sensus Ultra handshake packet (only version and serial supported 60 # at the moment) 61 HandshakeDump = namedtuple('HandshakeDump', 'ver1 ver2 serial time') 62 FMT_HANDSHAKE = '<bbHL' 63 64 DiveHeader = namedtuple('DiveHeader', 'time interval threshold' 65 ' endcount averaging') 66 # 4 bytes of padding, it is start of the header (0x00000000) 67 FMT_DIVE_HEADER = '<4xL4H'

68 69 # 70 # libdivecomputer data structures and constants 71 # 72 73 # see buffer.c, buffer.h 74 -class DCBuffer(ct.Structure):

75 _fields_ = [ 76 ('data', ct.POINTER(ct.c_char)), 77 ('capacity', ct.c_size_t), 78 ('offset', ct.c_size_t), 79 ('size', ct.c_size_t), 80 ]

81 82 # see parser.h:parser_sample_type_t 83 SampleType = namedtuple('SampleType', 'time depth pressure temperature' \ 84 ' event rbt heartbeat bearing vendor')._make(range(9))

85 86 87 -class Pressure(ct.Structure):

88 _fields_ = [ 89 ('tank', ct.c_uint), 90 ('value', ct.c_double), 91 ]

92

93 94 -class Event(ct.Structure):

95 _fields_ = [ 96 ('type', ct.c_uint), 97 ('time', ct.c_uint), 98 ('flags', ct.c_uint), 99 ('value', ct.c_uint), 100 ]

101

102 103 -class Vendor(ct.Structure):

104 _fields_ = [ 105 ('type', ct.c_uint), 106 ('size', ct.c_uint), 107 # fixme: (segv): ('data', ct.c_void_p), 108 ]

109

110 111 -class SampleValue(ct.Union):

112 _fields_ = [ 113 ('time', ct.c_uint), 114 ('depth', ct.c_double), 115 ('pressure', Pressure), 116 ('temperature', ct.c_double), 117 # fixme: segv with lines below 118 # ('event', Event), 119 # ('rbt', ct.c_uint), 120 # ('heartbeat', ct.c_uint), 121 # ('bearing', ct.c_uint), 122 # ('vendor', Vendor), 123 ]

124 125 126 # dive and sample data callbacks 127 FuncDive = ct.CFUNCTYPE(ct.c_uint, ct.POINTER(ct.c_char), ct.c_uint, 128 ct.POINTER(ct.c_char), ct.c_uint, ct.c_void_p) 129 FuncSample = ct.CFUNCTYPE(None, ct.c_int, SampleValue, ct.c_void_p) 130 131 132 @kc.inject(DeviceDriver, id='su', name='Sensus Ultra Driver', 133 models=('Sensus Ultra',))

134 -class SensusUltraDriver(object):

135 """ 136 Sensus Ultra dive logger driver. 137 """

138 - def __init__(self, dev, lib):

139 self.dev = dev 140 self.lib = lib

141 142 143 @staticmethod

144 - def scan(port=None):

145 """ 146 Look for Reefnet Sensus Ultra dive logger connected to one of USB 147 ports. 148 149 Library `libdivecomputer` is used, therefore no scanning and port 150 shall be specified. 151 """ 152 lib = ct.CDLL('libdivecomputer.so.0') 153 154 dev = ct.c_void_p() 155 rc = 0 156 if port is not None: 157 rc = lib.reefnet_sensusultra_device_open(ct.byref(dev), 158 port.encode()) 159 if rc == 0: 160 drv = SensusUltraDriver(dev, lib) 161 log.debug('found Reefnet Sensus Ultra driver using' \ 162 ' libdivecomputer library on port {}'.format(port)) 163 yield drv 164 else: 165 log.debug('libdc error: {}'.format(rc))

166 167

168 - def version(self):

169 """ 170 Read Reefnet Sensus Ultra version and serial number. 171 """ 172 173 sd = ct.create_string_buffer(SIZE_MEM_SENSE) 174 rc = self.lib.reefnet_sensusultra_device_sense(self.dev, sd, SIZE_MEM_SENSE) 175 if rc != 0: 176 raise DeviceError('Device communication error') 177 178 hd = ct.create_string_buffer(SIZE_MEM_HANDSHAKE) 179 rc = self.lib.reefnet_sensusultra_device_get_handshake(self.dev, hd, SIZE_MEM_HANDSHAKE) 180 if rc != 0: 181 raise DeviceError('Device communication error') 182 183 # take 8 bytes for now (version, serial and time) 184 dump = _handshake(hd.raw) 185 return 'Sensus Ultra %d.%d' % (dump.ver2, dump.ver1)

186 187 188 189 @kc.inject(DataParser, id='su', data=())

190 -class SensusUltraDataParser(object):

191 """ 192 Reefnet Sensus Ultra dive logger data parser. 193 """ 194

195 - def dump(self):

196 """ 197 Download Sensus Ultra 198 199 - handshake packet 200 - user data 201 - data of all dive profiles 202 """ 203 dev = self.driver.dev 204 lib = self.driver.lib 205 206 hd = ct.create_string_buffer(SIZE_MEM_HANDSHAKE) 207 ud = ct.create_string_buffer(SIZE_MEM_USER) 208 dd = ct.cast(lib.dc_buffer_new(SIZE_MEM_DATA), ct.POINTER(DCBuffer)) 209 210 log.debug('loading user data') 211 rc = lib.reefnet_sensusultra_device_read_user(dev, ud, SIZE_MEM_USER) 212 if rc != 0: 213 log.debug('read error rc = {}'.format(rc)) 214 raise DeviceError('Device communication error') 215 216 log.debug('loading handshake data') 217 rc = lib.reefnet_sensusultra_device_get_handshake(dev, hd, SIZE_MEM_HANDSHAKE) 218 if rc != 0: 219 log.debug('read error rc = {}'.format(rc)) 220 raise DeviceError('Device communication error') 221 222 log.debug('loading dive data') 223 rc = lib.device_dump(dev, dd) 224 if rc != 0: 225 log.debug('read error rc = {}'.format(rc)) 226 raise DeviceError('Device communication error') 227 228 return hd.raw + ud.raw + dd.contents.data[:dd.contents.size]

229 230

231 - def dives(self, dump):

232 """ 233 Convert Reefnet Sensus Ultra dive data into UDDF format. 234 """ 235 #dev = self.driver.dev 236 #lib = self.driver.lib 237 lib = ct.CDLL('libdivecomputer.so.0') 238 239 parser = ct.c_void_p() 240 rc = lib.reefnet_sensusultra_parser_create(ct.byref(parser)) 241 if rc != 0: 242 raise DeviceError('Cannot create data parser') 243 244 hd = dump.data[:END_HANDSHAKE] 245 assert len(hd) == SIZE_MEM_HANDSHAKE, len(hd) 246 hdp = _handshake(hd) 247 248 ud = dump.data[START_USER : END_USER] 249 assert len(ud) == SIZE_MEM_USER, len(ud) 250 251 dd = ct.create_string_buffer(SIZE_MEM_DATA) 252 dd.raw = dump.data[START_DATA:] 253 assert len(dd.raw) == SIZE_MEM_DATA, len(dd.raw) 254 255 # boot time = host time - device time (sensus time) 256 btime = time.mktime(dump.datetime.timetuple()) - hdp.time 257 258 dq = Queue(10) 259 parse_dive = partial(self.parse_dive, 260 parser=parser, boot_time=btime, dives=dq) 261 f = FuncDive(parse_dive) 262 extract_dives = partial(lib.reefnet_sensusultra_extract_dives, 263 None, dd, SIZE_MEM_DATA, f, None) 264 265 return _iterate(dq, extract_dives)

266 267

268 - def gases(self, data):

269 """ 270 Return empty tuple - no gas information stored by Sensus Ultra. 271 272 :Parameters: 273 data 274 Sensus Ultra data. 275 """ 276 return ()

277 278

279 - def version(self, data):

280 """ 281 Get Sensus Ultra version from raw data. 282 283 :Parameters: 284 data 285 Raw dive computer data. 286 """ 287 status = _handshake(data) 288 return 'Sensus Ultra %d.%d' % (status.ver2, status.ver1)

289 290

291 - def parse_dive(self, buffer, size, fingerprint, fsize, pdata, parser, 292 boot_time, dives):

293 """ 294 Callback used by libdivecomputer's library function to extract 295 dives from a device and put it into dives queue. 296 297 :Parameters: 298 buffer 299 Buffer with binary dive data. 300 size 301 Size of buffer dive data. 302 fingerprint 303 Fingerprint buffer. 304 fsize 305 Size of fingerprint buffer. 306 pdata 307 Parser user data (nothing at the moment). 308 parser 309 libdivecomputer parser instance. 310 boot_time 311 Sensus Ultra boot time. 312 dives 313 Queue of dives to be consumed by caller. 314 """ 315 lib = ct.CDLL('libdivecomputer.so.0') 316 lib.parser_set_data(parser, buffer, size) 317 318 header = _dive_header(buffer) 319 log.debug('parsing dive: {0}'.format(header)) 320 321 # dive time is in seconds since boot time 322 # interval is substracted due to depth=0, time=0 sample injection 323 st = datetime.fromtimestamp(boot_time - header.interval + header.time) 324 log.debug('got dive time: {0}'.format(st)) 325 326 samples = [] 327 parse_sample = partial(self.parse_sample, 328 dive_header=header, 329 sdata={}, 330 sq=samples) 331 332 f = FuncSample(parse_sample) 333 lib.parser_samples_foreach(parser, f, None) 334 335 log.debug('removing {} endcount samples'.format(header.endcount)) 336 del samples[-header.endcount:] 337 338 # dive summary after endcount removal 339 max_depth = max(samples, key=operator.attrgetter('depth')).depth 340 min_temp = min(samples, key=operator.attrgetter('temp')).temp 341 duration = samples[-1].time + header.interval 342 343 # each dive starts below DiveHeader.threshold, therefore inject 344 # first sample required by UDDF 345 samples.insert(0, kd.Sample(depth=0.0, time=0)) 346 347 # each dive ends at about DiveHeader.threshold depth, therefore 348 # inject last sample required by UDDF 349 samples.append(kd.Sample(depth=0.0, time=duration)) 350 351 # finally, create dive data 352 dive = kd.Dive(datetime=st, depth=max_depth, duration=duration, 353 temp=min_temp, profile=samples) 354 355 try: 356 dives.put(dive, timeout=30) 357 except Full: 358 log.error('could not parse dives due to internal queue timeout') 359 return 0 360 361 return 1

362 363

364 - def parse_sample(self, st, sample, pdata, dive_header, sdata, sq):

365 """ 366 Convert dive samples data generated with libdivecomputer library 367 into UDDF waypoint structure. 368 369 :Parameters: 370 st 371 Sample type as specified in parser.h. 372 sample 373 Sample data. 374 pdata 375 Parser user data (nothing at the moment). 376 dive_header 377 Dive header of parsed dive. 378 sdata 379 Temporary sample data. 380 sq 381 List of samples. 382 """ 383 # depth is the last sample type generated by libdivecomputer, 384 # create the waypoint then 385 if st == SampleType.time: 386 sdata['time'] = sample.time 387 elif st == SampleType.temperature: 388 sdata['temp'] = round(C2K(sample.temperature), 1) 389 elif st == SampleType.depth: 390 # Sensus Ultra might return negative values near 0, so use max 391 sdata['depth'] = round(max(sample.depth, 0), 1) 392 393 s = kd.Sample(**sdata) 394 log.debug('got sample {}'.format(sdata)) 395 396 sq.append(s) 397 sdata.clear() # clear temporary data 398 else: 399 log.warn('unknown sample type', st) 400 return 1

401

402 403 -def _handshake(data):

404 """ 405 Convert binary data into HandshakeDump structure. 406 407 :Parameters: 408 data 409 Binary data. 410 411 """ 412 return HandshakeDump._make(unpack(FMT_HANDSHAKE, data[:8]))

413

414 415 -def _dive_header(data):

416 """ 417 Convert binary data into DiveHeader structure. 418 419 :Parameters: 420 data 421 Dive binary data. 422 """ 423 return DiveHeader._make(unpack(FMT_DIVE_HEADER, data[:16]))

424

425 426 -def _iterate(queue, f):

427 """ 428 Create iterator for stateful function. 429 430 Stateful function pushes items into to the queue, while this function 431 pull items from the queue and returns them one by one. 432 433 :Parameters: 434 queue 435 Queue holding stateful function items. 436 f 437 Stateful function. 438 """ 439 with ThreadPoolExecutor(max_workers=1) as e: 440 fn = e.submit(f) 441 442 while not (fn.done() and queue.empty()): 443 try: 444 n = queue.get(timeout=1) 445 yield n 446 except Empty: 447 if not fn.done(): 448 log.warn('su driver possible queue miss') 449 450 if fn.result() == 0: 451 raise StopIteration() 452 else: 453 raise DeviceError('Failed to extract data properly')

454 455 # vim: sw=4:et:ai 456

Source Code for Module kenozooid.driver.su