SCX/Reader.pm

   1 #!/usr/bin/perl -w
   2
   3 package SCX::Reader;
   4
   5 use strict;
   6
   7 use Time::HiRes qw(gettimeofday tv_interval);
   8 use FileHandle;
   9 use IO::Handle;
  10 use POSIX;
  11 use SCX::CRC;
  12
  13 our $PACKET_SIZE = 9; # 9 bytes + 0x05
  14 our $LOG_ROTATE  = 600;
  15
  16 sub new {
  17         my ($class, $args) = @_;
  18
  19         my $portname = $args->{portname}
  20                 or die "portname not specified";
  21
  22         system 'stty', '-F', $portname, '115200', 'raw';
  23         if ($?) {
  24                 die "stty died with code $? (no permissions?)";
  25         }
  26
  27         sysopen(my $fh, $portname, O_RDONLY|O_NONBLOCK)
  28                 or die "Can't open $portname: $!";
  29
  30         my $logfile = $args->{logfile};
  31         my $log_gen = 0;
  32
  33         open my $logfh, '>', "$logfile.$log_gen"
  34         or die "Can't open $logfile.$log_gen: $!";
  35
  36         my $now = gettimeofday;
  37
  38         my $self = {
  39                 portname  => $portname,
  40                 fh        => $fh,
  41                 logfile   => $logfile,
  42                 logfh     => $logfh,
  43                 log_gen   => $log_gen,
  44                 log_start => $now,
  45                 starttime => $now,
  46                 track     => $args->{track},
  47                 bytes     => [],
  48         };
  49
  50         bless $self, $class;
  51
  52         $self->track->reset;
  53
  54         return $self;
  55 }
  56
  57 sub fh { return shift->{fh}; }
  58
  59 sub track { return shift->{track}; }
  60
  61 sub read {
  62         my ($self) = @_;
  63
  64         my $data;
  65         my $bytes_read = sysread $self->fh, $data, $PACKET_SIZE;
  66         die "Read error on $self->{portname}: $!"
  67                 if !$bytes_read;
  68
  69         $self->{last_read_time} = gettimeofday;
  70
  71         my @bytes = unpack("C*", $data);
  72
  73         # print join(' ', map { sprintf(" %02x", $_) } @bytes), "\n";
  74         push @{ $self->{bytes} }, @bytes;
  75         @bytes = @{ $self->{bytes} };
  76
  77         my @bad_bytes;
  78
  79         while (@bytes > $PACKET_SIZE) {
  80                 if ($bytes[0] != 0x55) {
  81                         push @bad_bytes, shift @bytes;
  82                         next;
  83                 }
  84                 my $cmd = $bytes[1];
  85
  86                 if ($bytes[$PACKET_SIZE] != 0x05
  87                         || SCX::CRC::digest(@bytes[0..$PACKET_SIZE-2])
  88                                 != $bytes[$PACKET_SIZE-1]) {
  89                         push @bad_bytes, shift @bytes;
  90                         next;
  91                 }
  92
  93                 if (@bad_bytes) { # Report previous bad bytes first
  94                         $self->log_bytes(\@bad_bytes, "Cannot parse packet");
  95                         @bad_bytes = ();
  96                 }
  97
  98                 my @packet = splice @bytes, 0, $PACKET_SIZE+1;
  99                 my $rv = $self->handle_packet(@packet);
 100                 $self->log_bytes(\@packet, $rv);
 101                 $self->track->packet_received($self->{last_read_time});
 102         }
 103         if (@bad_bytes) {
 104                 while (@bytes && $bytes[0] != 0x55) {
 105                         push @bad_bytes, shift @bytes;
 106                 }
 107                 $self->log_bytes(\@bad_bytes, "cannot parse packet");
 108         }
 109
 110         @{ $self->{bytes} } = @bytes;
 111 }
 112
 113 sub log_bytes {
 114         my ($self, $bytes, $msg) = @_;
 115
 116         return if !@$bytes;
 117
 118         $msg = defined $msg ? ' # ' . $msg : '';
 119
 120         my $now = $self->{last_read_time};
 121
 122         if ($now - $self->{log_start} >= $LOG_ROTATE) {
 123                 close $self->{logfh};
 124                 $self->{log_gen} = $self->{log_gen} ? 0 : 1;
 125                 open my $fh, '>', $self->{logfile} . '.' . $self->{log_gen}
 126                         or die "Can't open $self->{logfile}.$self->{log_gen}: $!";
 127                 $self->{logfh} = $fh;
 128                 $self->{log_start} = $now;
 129         }
 130
 131         $self->{logfh}->print(sprintf('% 10.3f', $now - $self->{starttime}),
 132                 (map { sprintf(" %02x", $_) } @$bytes),
 133                 $msg, "\n");
 134         $self->{logfh}->flush;
 135 }
 136
 137 our %COMMANDS = (
 138         0xAA => \&bus_free_time_packet,
 139         0xCC => \&car_programming_packet,
 140         0xD0 => \&reset_packet,
 141         0xD3 => \&standings_packet,
 142         0xD4 => \&lap_time_packet,
 143         0xD5 => \&race_setup_packet,
 144         0xD6 => \&fuel_level_packet,
 145         0xD7 => \&brake_set_packet,
 146         0xDB => \&qualification_packet,
 147         0xDC => \&end_of_race_packet,
 148         0xDD => \&race_start_packet,
 149         0xDE => \&display_change_packet,
 150         0xEE => \&finish_line_packet,
 151         0xFF => \&controller_status_packet,
 152 );
 153
 154 sub handle_packet {
 155         my ($self, @data) = @_;
 156
 157         my $cmd = $data[1];
 158         my @args = @data[2..7];
 159
 160         my $sub = $COMMANDS{$cmd};
 161         return "Unknown packet"
 162                 if !defined $sub;
 163
 164         return &$sub($self, @args);
 165 }
 166
 167 sub bus_free_time_packet {
 168         my ($self, @bytes) = @_;
 169
 170         my $msg = 'Strange bus free time packet'
 171                 if $bytes[2] != 0xF0
 172                 || $bytes[3] != 0xF0
 173                 || $bytes[4] != 0xF0
 174                 || $bytes[5] != 0xF0;
 175
 176         return $msg; # No need to handle this, I think
 177 }
 178
 179 sub car_programming_packet {
 180         my ($self, @bytes) = @_;
 181
 182         my $msg = 'Strange car programming packet'
 183                 if ($bytes[0] & 0xF8) != 0 || ($bytes[0] & 0x07) > 5
 184                 || $bytes[1] != 0xFE
 185                 || $bytes[2] != 0xFF
 186                 || $bytes[3] != 0xFF
 187                 || $bytes[4] != 0xFF
 188                 || $bytes[5] != 0xFF;
 189
 190         return $msg; # No need to handle this
 191 }
 192
 193 sub reset_packet {
 194         my ($self, @bytes) = @_;
 195
 196         my $msg = 'Strange reset packet'
 197                 if $bytes[0] != 0xFF
 198                 || $bytes[3] != 0xAA
 199                 || $bytes[4] != 0xAA
 200                 || $bytes[5] != 0xAA;
 201
 202         $self->track->reset;
 203
 204         return $msg; # FIXME - to be implemented
 205 }
 206
 207 sub standings_packet {
 208         my ($self, @bytes) = @_;
 209
 210         my $msg = 'Strange standings packet'
 211                 if ($bytes[0] != 0xFF && ($bytes[0] & 0x07) > 5)
 212                 || ($bytes[1] != 0xFF && ($bytes[1] & 0x07) > 5)
 213                 || ($bytes[2] != 0xFF && ($bytes[2] & 0x07) > 5)
 214                 || ($bytes[3] != 0xFF && ($bytes[3] & 0x07) > 5)
 215                 || ($bytes[4] != 0xFF && ($bytes[4] & 0x07) > 5)
 216                 || ($bytes[5] != 0xFF && ($bytes[5] & 0x07) > 5);
 217
 218         my @standings;
 219
 220         push @standings, map { $_ != 0xFF ? $_ & (0x07) : () } @bytes;
 221
 222         return $msg; # FIXME - to be implemented
 223 }
 224
 225 sub lap_time_packet {
 226         my ($self, @bytes) = @_;
 227
 228         my $msg = 'Strange lap time packet'
 229                 if $bytes[0] > 5
 230                 || $bytes[1] & 0x01
 231                 || $bytes[2] & 0x01
 232                 || ($bytes[3] & 0xF0) != 0
 233                 || $bytes[4] & 0x01
 234                 || $bytes[5] & 0x01;
 235
 236 =comment
 237         # Moving to internal timekeeping
 238         my $nonzero = grep { $_ != 0 } @bytes;
 239
 240         my $car = $bytes[0];
 241         my $round = 256*$bytes[1] + $bytes[2]
 242                 + ($bytes[3] & 2 ? 256 : 0)
 243                 + ($bytes[3] & 1 ? 1 : 0);
 244         my $time = 256*$bytes[4] + $bytes[5]
 245                 + ($bytes[3] & 8 ? 256 : 0)
 246                 + ($bytes[3] & 4 ? 1 : 0);
 247         if ($time == 65535) {
 248                 $self->track->car($car)->enter_pit_lane;
 249         } else {
 250                 $time *= 0.01024;
 251
 252                 if ($nonzero) {
 253                         $self->track->car($car)->set_lap($round);
 254                         $self->track->car($car)->set_laptime($time);
 255                 } else {
 256                         # FIXME - probably reset race time or whatever
 257                         # all-zeros packet is sent after the race setup
 258                 }
 259         }
 260 =cut
 261
 262         return $msg;
 263 }
 264
 265
 266 sub race_setup_packet {
 267         my ($self, @bytes) = @_;
 268
 269         my $msg = 'Strange race setup packet'
 270                 if ($bytes[0] != 0x00 && $bytes[0] != 0xFF)
 271                 || $bytes[1] & 0xF0
 272                 || $bytes[2] & 0xF0
 273                 || $bytes[3] & 0xF0
 274                 || $bytes[4] != 0xFF
 275                 || $bytes[5] != 0xFF;
 276
 277         $self->track->race_setup($bytes[0] == 0x00
 278                 ? 0
 279                 : ($bytes[1] & 0x0F) * 256
 280                         + ($bytes[2] & 0x0F) * 16
 281                         + ($bytes[3] & 0x0F));
 282
 283         return $msg;
 284 }
 285
 286
 287 sub fuel_level_packet {
 288         my ($self, @bytes) = @_;
 289
 290         my $msg = 'Strange fuel_level packet'
 291                 if ($bytes[0] >> 4) > 8
 292                 || ($bytes[0] & 0x0F) > 8
 293                 || ($bytes[1] >> 4) > 8
 294                 || ($bytes[1] & 0x0F) > 8
 295                 || ($bytes[2] >> 4) > 8
 296                 || ($bytes[2] & 0x0F) > 8
 297                 || ($bytes[5] != 0xAA && $bytes[5] != 0xFF);
 298
 299         my @fuel = (
 300                 $bytes[0] >> 4, $bytes[0] & 0x0f,
 301                 $bytes[1] >> 4, $bytes[1] & 0x0f,
 302                 $bytes[2] >> 4, $bytes[2] & 0x0f,
 303         );
 304
 305         for my $car (0..5) {
 306                 $self->track->car($car)->set_fuel($fuel[$car]);
 307         }
 308
 309         return $msg;
 310 }
 311
 312
 313 sub brake_set_packet {
 314         my ($self, @bytes) = @_;
 315
 316         return 'Unexpected brake_set packet (should be in the pit lane only)';
 317 }
 318
 319
 320 sub qualification_packet {
 321         my ($self, @bytes) = @_;
 322
 323         my $msg = 'Strange qualification packet'
 324                 if $bytes[0] & 0xF0
 325                 || $bytes[1] & 0xF0
 326                 || $bytes[2] & 0xF0
 327                 || $bytes[3] > 5
 328                 || $bytes[4] != 0xFF
 329                 || $bytes[5] != 0xFF;
 330
 331         $self->track->qualification_start;
 332
 333         return $msg;
 334 }
 335
 336
 337 sub end_of_race_packet {
 338         my ($self, @bytes) = @_;
 339
 340         my $msg = 'Strange end_of_race packet'
 341                 if $bytes[0] != 0xFF
 342                 || $bytes[1] != 0xFF
 343                 || $bytes[2] != 0xFF
 344                 || $bytes[3] != 0xFF
 345                 || $bytes[4] != 0xFF
 346                 || $bytes[5] != 0xFF;
 347
 348         $self->track->race_end;
 349
 350         return $msg;
 351 }
 352
 353
 354 sub race_start_packet {
 355         my ($self, @bytes) = @_;
 356
 357         my $msg = 'Strange race_start packet'
 358                 if $bytes[0] != 0x00
 359                 || $bytes[1] != 0xAA
 360                 || $bytes[2] != 0xAA
 361                 || $bytes[3] != 0xAA
 362                 || $bytes[4] != 0xAA
 363                 || $bytes[5] != 0xAA;
 364
 365         $self->track->race_start;
 366
 367         return $msg;
 368 }
 369
 370
 371 sub display_change_packet {
 372         my ($self, @bytes) = @_;
 373
 374         my $msg = 'Strange display_change packet'
 375                 if $bytes[0] & 0xFE
 376                 || $bytes[1] != 0xFF
 377                 || $bytes[2] != 0xFF
 378                 || $bytes[3] != 0xFF
 379                 || $bytes[4] != 0xFF
 380                 || $bytes[5] != 0xFF;
 381
 382         return $msg; # FIXME - to be implemented
 383 }
 384
 385
 386 sub finish_line_packet {
 387         my ($self, @bytes) = @_;
 388
 389         my $fail;
 390         for my $byte (@bytes) {
 391                 $fail = 1
 392                         if $byte != 0xAA
 393                         && $byte != 0xE7
 394                         && $byte != 0xF0
 395                         && $byte != 0xFE
 396         }
 397
 398         my $msg = 'Strange finish_line packet'
 399                 if $fail;
 400
 401         my $regular = 1;
 402         my @cars_finished;
 403         for my $i (0..5) {
 404                 my $byte = $bytes[$i];
 405
 406                 $regular = 0
 407                         if $byte != 0xAA && $byte != 0xE7 && $byte != 0xFE;
 408
 409                 push @cars_finished, $i if $byte == 0xE7;
 410         }
 411
 412         $self->track->finish_line(
 413                 $self->{last_read_time},
 414                 $regular,
 415                 @cars_finished
 416         );
 417
 418         return $msg;
 419 }
 420
 421 sub controller_status_packet {
 422         my ($self, @bytes) = @_;
 423
 424         my $fail;
 425         for my $byte (@bytes) {
 426                 next if $byte == 0xAA;
 427                 $fail = 1
 428                         if ($byte & 0xC0) != 0xC0
 429                         || ($byte & 0x0F) > 12
 430         }
 431
 432         my $msg = 'Strange controller_status packet'
 433                 if $fail;
 434
 435         my @fuel = (
 436                 $bytes[1] >> 4, $bytes[1] & 0x0f,
 437                 $bytes[2] >> 4, $bytes[2] & 0x0f,
 438                 $bytes[3] >> 4, $bytes[3] & 0x0f,
 439         );
 440
 441         for my $car (0..5) {
 442                 my $byte = $bytes[$car];
 443
 444                 if ($byte == 0xAA) {
 445                         $self->track->car($car)->set_throttle(undef);
 446                         next;
 447                 }
 448
 449                 my $light = !($byte & 0x20);
 450                 my $backbutton = !($byte & 0x10);
 451                 my $throttle = $byte & 0x0f;
 452
 453                 $self->track->car($car)->set_throttle($throttle,
 454                         $self->{last_read_time});
 455                 $self->track->car($car)->set_light($light);
 456                 $self->track->car($car)->set_backbutton($backbutton);
 457         }
 458
 459         return $msg;
 460 }
 461
 462 1;
 463