From 429d195603305484766b6d568d8b522fce13d806 Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Sat, 1 Jun 2019 17:19:13 -0700
Subject: [PATCH] Few cleanups in ugv.cc

---
 main/ugv.cc | 27 +++++++++++++++++++++------
 1 file changed, 21 insertions(+), 6 deletions(-)

diff --git a/main/ugv.cc b/main/ugv.cc
index adb4393..2c96ade 100644
--- a/main/ugv.cc
+++ b/main/ugv.cc
@@ -8,6 +8,11 @@ namespace ugv {
 
 static const char *TAG = "ugv_main";
 
+//#define CALIBRATE
+
+static constexpr float PI =
+    3.1415926535897932384626433832795028841971693993751058209749445923078164062;
+
 constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
 constexpr float LOOP_HZ = 1000000.f / static_cast<float>(LOOP_PERIOD_US);
 constexpr float LOOP_PERIOD_S = 1 / LOOP_HZ;
@@ -108,11 +113,14 @@ void UGV::UpdateAHRS() {
 
 void UGV::DoDebugPrint() {
   auto &mpu = inputs_.mpu;
+#ifdef CALIBRATE
   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);
+#else
   ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", yaw_, pitch_, roll_);
   ESP_LOGV(TAG, "target angle: %f", angle_controller_.Setpoint());
+#endif
 }
 
 void UGV::ReadComms() {
@@ -155,7 +163,7 @@ void UGV::ReadComms() {
 void UGV::OnTick() {
   ESP_LOGV(TAG, "OnTick");
   int64_t time_us = esp_timer_get_time();
-  // float   time_s  = ((float)time_us) / 1e6;
+  float time_s = ((float)time_us) / 1e6;
   io->ReadInputs(inputs_);
   UpdateAHRS();
 
@@ -267,9 +275,10 @@ void UGV::OnTick() {
       break;
     }
     case UGV_State::STATE_TEST:
+#define BASIC_TEST
 #ifdef BASIC_TEST
-      outputs.left_motor = sinf(time_s * PI);
-      outputs.right_motor = cosf(time_s * PI);
+      outputs_.left_motor = sinf(time_s * PI);
+      outputs_.right_motor = cosf(time_s * PI);
 #else
       angle_controller_.Enable();
       angle_controller_.Setpoint(90.0);
@@ -292,8 +301,8 @@ void UGV::OnTick() {
 
   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;
   } else {
     float dleft = outputs_.left_motor - last_left_;
     float dright = outputs_.right_motor - last_right_;
@@ -319,7 +328,13 @@ void UGV::OnTick() {
   comms->status.set_state(next_state_);
   comms->Unlock();
 
-  if (time_us >= last_print_ + 500 * 1000) {
+#ifdef CALIBRATE
+  const long debug_print_period = 50 * 1000;
+#else
+  const long debug_print_period = 500 * 1000;
+#endif
+
+  if (time_us >= last_print_ + debug_print_period) {
     DoDebugPrint();
     last_print_ = time_us;
   }