Reader.pm: fixed packet size, refactor logging.

[slotcarman.git] / SCX / Reader.pm

diff --git a/SCX/Reader.pm b/SCX/Reader.pm
index 1b7c44f40203c49f2833b1816feda3ee46d2f4d0..23f33eab3c38429eab22fa5a27a526cb672fc8f7 100644 (file)
--- a/SCX/Reader.pm
+++ b/SCX/Reader.pm
@@ -6,7 +6,7 @@ use Time::HiRes qw(gettimeofday tv_interval);
 use FileHandle;
 use SCX::CRC;
 
-our $PACKET_SIZE = 10;
+our $PACKET_SIZE = 9; # 9 bytes + 0x05
 our $LOG_ROTATE  = 600;
 
 sub new {
@@ -61,54 +61,63 @@ sub read {
        die "Read error on $self->{portname}: $!"
                if !$bytes_read;
 
-       my $now = gettimeofday;
-       if ($now - $self->{log_start} >= $LOG_ROTATE) {
-               close $self->{logfh};
-               $self->{log_gen} = $self->{log_gen} ? 0 : 1;
-               open my $fh, '>', $logfile . '.' . $self->{log_gen}
-                       or die "Can't open $logfile.$self->{log_gen}: $!";
-               $self->{logfh} = $fh;
-               $self->{log_start} = $now;
-       }
-
        my @bytes = unpack("C*", $data);
 
-       $self->{logfh}->print(sprintf('% 10.3f', $now - $self->{starttime}),
-               (map { sprintf(" %02x", $_) } @bytes),
-               "\n");
-
        push @{ $self->{bytes} }, @bytes;
        @bytes = @{ $self->{bytes} };
 
        my @bad_bytes;
 
-       while (@bytes >= $PACKET_SIZE) {
-               if ($bytes[0] != 0x55 || $bytes[9] != 0x05
-                       || SCX::CRC::digest(@bytes[0..7]) != $bytes[8]) {
+       while (@bytes > $PACKET_SIZE) {
+               if ($bytes[0] != 0x55) {
                        push @bad_bytes, shift @bytes;
                        next;
                }
+               my $cmd = $bytes[1];
 
-               if (@bad_bytes) {
-                       $self->{logfh}->print("Cannot parse bytes",
-                               (map { sprintf(' %02x', $_) } @bad_bytes),
-                               "\n");
+               if ($bytes[$PACKET_SIZE] != 0x05
+                       || SCX::CRC::digest(@bytes[0..$PACKET_SIZE-2])
+                               != $bytes[$PACKET_SIZE-1]) {
+                       push @bad_bytes, shift @bytes;
+                       next;
+               }
+               
+               if (@bad_bytes) { # Report previous bad bytes first
+                       $self->log_bytes(\@bad_bytes, "Cannot parse packet");
                        @bad_bytes = ();
                }
 
-               $self->{logfh}->print("Callback\n");
-               &{ $self->{callback} }(@bytes[1..7]);
-               splice @bytes, 0, 10;
-       }
-       if (@bad_bytes) {
-               $self->{logfh}->print("Cannot parse bytes",
-                       (map { sprintf(' %02x', $_) } @bad_bytes),
-                       "\n");
-               @bad_bytes = ();
+               my @packet = splice @bytes, 0, $PACKET_SIZE+1;
+               my $rv = &{ $self->{callback} }(@packet[1..$PACKET_SIZE]);
+               $self->log_bytes(@packet, $rv);
        }
+       $self->log_bad_bytes(\@bad_bytes, "Cannot parse packet");
 
        @{ $self->{bytes} } = @bytes;
 }
 
+sub log_bytes {
+       my ($self, $bytes, $msg) = @_;
+
+       return if !@$bytes;
+
+       $msg = defined $msg ? ' # ' . $msg : '';
+
+       my $now = gettimeofday;
+
+       if ($now - $self->{log_start} >= $LOG_ROTATE) {
+               close $self->{logfh};
+               $self->{log_gen} = $self->{log_gen} ? 0 : 1;
+               open my $fh, '>', $logfile . '.' . $self->{log_gen}
+                       or die "Can't open $logfile.$self->{log_gen}: $!";
+               $self->{logfh} = $fh;
+               $self->{log_start} = $now;
+       }
+
+       $self->{logfh}->print(sprintf('% 10.3f', $now - $self->{starttime}),
+               (map { sprintf(" %02x", $_) } @bytes),
+               $msg, "\n");
+}
+
 1;