From b72bbba25e5172dac4d1f54a6a78366cfd9fd0d2 Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Sun, 2 Jun 2019 10:29:32 -0700
Subject: [PATCH] Add CRC checking to python tools

---
 tools/config.yml       |  8 +++----
 tools/requirements.txt |  1 +
 tools/ugv.py           | 51 ++++++++++++++++++++++++++++++------------
 tools/ugv_cmd.py       |  7 ++++++
 tools/ugv_to_ground.py |  2 +-
 5 files changed, 50 insertions(+), 19 deletions(-)

diff --git a/tools/config.yml b/tools/config.yml
index 1d7151b..590ed1c 100644
--- a/tools/config.yml
+++ b/tools/config.yml
@@ -1,12 +1,12 @@
 REVISION: 1
 angle_pid:
-  kp: 0.06
-  ki: 0.01
+  kp: 0.04
+  ki: 0.00 #0001
   kd: 0.4
-  max_output: 0.6
+  max_output: 0.4
   max_i_error: 15.0
 
 min_target_dist: 1.0
 min_flip_pitch: 90.0
-drive_power: 0.4
+drive_power: 0.3
 mag_declination: 11.5
diff --git a/tools/requirements.txt b/tools/requirements.txt
index d30b2c3..3b1cf35 100644
--- a/tools/requirements.txt
+++ b/tools/requirements.txt
@@ -3,6 +3,7 @@ chardet==3.0.4
 idna==2.8
 netifaces==0.10.6
 protobuf==3.7.1
+PyCRC==1.21
 pyserial==3.4
 python-engineio==3.5.2
 python-socketio==4.0.2
diff --git a/tools/ugv.py b/tools/ugv.py
index 25727ea..6283e6d 100755
--- a/tools/ugv.py
+++ b/tools/ugv.py
@@ -8,6 +8,8 @@ from threading import Thread
 import time
 import binascii
 from base64 import b64decode, b64encode
+from PyCRC.CRC32 import CRC32
+import struct
 
 import messages_pb2 as messages
 from google.protobuf.message import Message
@@ -29,9 +31,13 @@ class UGVComms:
         self.last_status_time = None
         self.rx_thread = None
         self.is_running = False
+        self.log_file = None
 
     def write_base64(self, data: bytes):
-        encoded = b64encode(data)
+        crc = CRC32().calculate(data)
+        data_with_checksum = bytearray(data)
+        data_with_checksum.extend(struct.pack('<L', crc))
+        encoded = b64encode(data_with_checksum)
         self.ser.write(encoded)
         self.ser.write(b'\n')
 
@@ -80,8 +86,16 @@ class UGVComms:
             log.warning("read bad data: %s", data)
             self.ser.flush()
             return None
+        if len(decoded) < 4:
+            log.warning('Message too short ({} bytes)'.format(len(decoded)))
+            return None
+        msgcrc, = struct.unpack('<L', decoded[-4:])
+        calccrc = CRC32().calculate(decoded[:-4])
+        if msgcrc != calccrc:
+            log.warning('Checksum did not match ({} != {})'.format(msgcrc, calccrc))
+            return None
         msg = messages.UGV_Message()
-        msg.ParseFromString(decoded)
+        msg.ParseFromString(decoded[:-4])
         return msg
 
     def process_message(self, msg: messages.UGV_Message):
@@ -90,6 +104,8 @@ class UGVComms:
         log.debug("received UGV message: %s", msg)
         if self.on_msg_received:
             self.on_msg_received(msg)
+        if self.log_file:
+            print('[{}] UGV_Message: {}'.format(time.strftime('%Y-%b-%d %H:%M:%S'), msg), file=self.log_file)
         if msg.HasField("command_ack"):
             with self.ack_cv:
                 self.msg_acks.append(msg.command_ack)
@@ -98,7 +114,7 @@ class UGVComms:
             self.last_status = msg.status
             self.last_status_time = time.time()
         else:
-            log.warn("unknown UGV message: %s", msg)
+            log.warning("unknown UGV message: %s", msg)
 
     def start(self):
         if self.is_running:
@@ -118,19 +134,26 @@ class UGVComms:
         self.rx_thread.join()
         return True
 
+    def save_logs(self, file):
+        self.log_file = open(file, mode='a')
+
     def __rx_thread_entry(self):
-        while self.is_running and self.ser.is_open:
-            try:
-                msg = self.read_message()
-                self.process_message(msg)
-            except serial.SerialException:
-                if not self.ser.is_open or not self.is_running:  # port was probably just closed
+        try:
+            while self.is_running and self.ser.is_open:
+                try:
+                    msg = self.read_message()
+                    self.process_message(msg)
+                except serial.SerialException:
+                    if not self.ser.is_open or not self.is_running:  # port was probably just closed
+                        return
+                    log.error("serial error", exc_info=True)
                     return
-                log.error("serial error", exc_info=True)
-                return
-            except Exception:
-                log.error("error reading message", exc_info=True)
-                continue
+                except Exception:
+                    log.error("error reading message", exc_info=True)
+                    continue
+        finally:
+            if self.log_file:
+                self.log_file.close()
 
 
 def main():
diff --git a/tools/ugv_cmd.py b/tools/ugv_cmd.py
index 4126414..2dfabca 100755
--- a/tools/ugv_cmd.py
+++ b/tools/ugv_cmd.py
@@ -155,6 +155,13 @@ class UGV_CLI:
         self.ugv.write_command(cmd)
         print("Received ping response")
 
+    @cli_cmd(names=["save_logs", "sl"], description="Save logs to a file")
+    def save_logs(self, file=None):
+        if file is None:
+            file = 'ugv_log.txt'
+        self.ugv.save_logs(file)
+        print("Saving logs to {}".format(file))
+
     @staticmethod
     def find_command(name):
         for cmd in cli_commands:
diff --git a/tools/ugv_to_ground.py b/tools/ugv_to_ground.py
index 3db8ef9..0aca139 100755
--- a/tools/ugv_to_ground.py
+++ b/tools/ugv_to_ground.py
@@ -20,7 +20,7 @@ def ping_thread_entry(ugv_cli):
     while ugv_cli.is_running and ugv_cli.ugv.ser.is_open:
         try:
             ugv_cli.get_status()
-            time.sleep(10.0)
+            time.sleep(5.0)
         except IOError as e:
             log.error("Error pinging UGV: {}".format(e))