From 8f3fdd137aceb03476099ddac42b4069959b6259 Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Sun, 2 Jun 2019 10:29:22 -0700
Subject: [PATCH] Try to fix more comms issues

---
 main/MadgwickAHRS.cpp |  2 +-
 main/ugv.cc           | 14 +++++++++++---
 main/ugv.hh           |  1 +
 main/ugv_comms.cc     | 29 +++++++++++++++++++++++++----
 4 files changed, 38 insertions(+), 8 deletions(-)

diff --git a/main/MadgwickAHRS.cpp b/main/MadgwickAHRS.cpp
index c78a68e..5a2c61d 100644
--- a/main/MadgwickAHRS.cpp
+++ b/main/MadgwickAHRS.cpp
@@ -27,7 +27,7 @@
 // Definitions
 
 #define sampleFreqDef 512.0f  // sample frequency in Hz
-#define betaDef 0.4f          // 2 * proportional gain
+#define betaDef 0.2f          // 2 * proportional gain
 
 //============================================================================================
 // Functions
diff --git a/main/ugv.cc b/main/ugv.cc
index 2c96ade..f826e30 100644
--- a/main/ugv.cc
+++ b/main/ugv.cc
@@ -118,6 +118,9 @@ void UGV::DoDebugPrint() {
            mpu.accel.x, mpu.accel.y, mpu.accel.z, mpu.gyro_rate.x,
            mpu.gyro_rate.y, mpu.gyro_rate.z, mpu.mag.x, mpu.mag.y, mpu.mag.z);
 #else
+  ESP_LOGD(TAG, "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)",
+           mpu.accel.x, mpu.accel.y, mpu.accel.z, mpu.gyro_rate.x,
+           mpu.gyro_rate.y, mpu.gyro_rate.z, mpu.mag.x, mpu.mag.y, mpu.mag.z);
   ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", yaw_, pitch_, roll_);
   ESP_LOGV(TAG, "target angle: %f", angle_controller_.Setpoint());
 #endif
@@ -297,12 +300,13 @@ void UGV::OnTick() {
     outputs_.right_motor = drive_power + angle_pwr;
   }
 
-  constexpr float MAX_ACCEL = 8 * LOOP_PERIOD_S;
+  constexpr float MAX_ACCEL = 16 * LOOP_PERIOD_S;
 
   if (inputs_.mpu.gyro_rate.x == 0 && inputs_.mpu.gyro_rate.y == 0 &&
       inputs_.mpu.gyro_rate.z == 0) {
-//    outputs_.left_motor = 0;
-//    outputs_.right_motor = 0;
+    outputs_.left_motor = 0;
+    outputs_.right_motor = 0;
+    did_miss_mpu_ = true;
   } else {
     float dleft = outputs_.left_motor - last_left_;
     float dright = outputs_.right_motor - last_right_;
@@ -335,6 +339,10 @@ void UGV::OnTick() {
 #endif
 
   if (time_us >= last_print_ + debug_print_period) {
+    if (did_miss_mpu_) {
+      ESP_LOGW(TAG, "no MPU data, disabling");
+      did_miss_mpu_ = false;
+    }
     DoDebugPrint();
     last_print_ = time_us;
   }
diff --git a/main/ugv.hh b/main/ugv.hh
index e6bfa64..9ae3628 100644
--- a/main/ugv.hh
+++ b/main/ugv.hh
@@ -55,6 +55,7 @@ class UGV {
   bool is_still_;
   float last_left_;
   float last_right_;
+  bool did_miss_mpu_;
 
   void UpdateAHRS();
   void DoDebugPrint();
diff --git a/main/ugv_comms.cc b/main/ugv_comms.cc
index 11dc706..12ec0b5 100644
--- a/main/ugv_comms.cc
+++ b/main/ugv_comms.cc
@@ -3,6 +3,7 @@
 
 #include <esp_log.h>
 #include <mbedtls/base64.h>
+#include <rom/crc.h>
 
 #include "messages.pb.h"
 
@@ -215,10 +216,24 @@ void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) {
     delete[] decoded;
     return;
   }
+  if (decoded_len < 4) {
+    ESP_LOGE(TAG, "message too short (%d bytes)", decoded_len);
+    delete[] decoded;
+    return;
+  }
+
+  uint32_t calccrc = crc32_le(0, decoded, decoded_len - 4);
+  uint32_t msgcrc;
+  memcpy(&msgcrc, decoded + decoded_len - 4, 4);
+  if (calccrc != msgcrc) {
+    ESP_LOGW(TAG, "crc did not match (message %u, calculated %u)", msgcrc, calccrc);
+    delete[] decoded;
+    return;
+  }
 
   GroundMessage ground_message;
 
-  bool pb_ret = ground_message.ParseFromArray(decoded, decoded_len);
+  bool pb_ret = ground_message.ParseFromArray(decoded, decoded_len - 4);
   delete[] decoded;
   if (!pb_ret) {
     ESP_LOGE(TAG, "rx invalid protobuf");
@@ -316,17 +331,23 @@ esp_err_t CommsClass::SendPacket(const uint8_t *data, size_t size) {
     ESP_LOGE(TAG, "SendPacket size too long: %d", size);
     return ESP_ERR_INVALID_SIZE;
   }
-  size_t encoded_size = ((size + 6) / 3) * 4;
+  size_t fullsize = size + 4;
+  uint8_t *data_with_crc = new uint8_t[fullsize];
+  uint32_t crc = crc32_le(0, data, size);
+  ESP_LOGV(TAG, "Calculated crc value: %u", crc);
+  memcpy(data_with_crc, data, size);
+  memcpy(data_with_crc + size, &crc, 4);
+  size_t encoded_size = ((fullsize + 6) / 3) * 4;
   uint8_t *encoded = new uint8_t[encoded_size];
   size_t encoded_len;
   int ret =
-      mbedtls_base64_encode(encoded, encoded_size, &encoded_len, data, size);
+      mbedtls_base64_encode(encoded, encoded_size, &encoded_len, data_with_crc, fullsize);
+  delete[] data_with_crc;
   if (ret != 0) {
     delete[] encoded;
     ESP_LOGE(TAG, "base64 encode error: %d", ret);
     return ESP_FAIL;
   }
-  // TODO: checksum
   lora.WriteLn(encoded, encoded_len);
   delete[] encoded;
   return ESP_OK;