create main loop

main/ugv_comms.cc

@@ -12,8 +12,6 @@
 namespace ugv {
 namespace comms {
 
-CommsClass Comms;
-
 static const char *TAG = "ugv_comms";
 
 CommsClass::CommsClass()
@@ -167,7 +165,7 @@ void CommsClass::RunTask() {
     if (ret != ESP_OK) {
       ESP_LOGE(TAG, "error sending packet: %d", ret);
     } else {
-      ESP_LOGI(TAG, "lora wrote UGV_Message packet");
+      ESP_LOGV(TAG, "lora wrote UGV_Message packet");
     }
 
     current_tick = xTaskGetTickCount();

main/ugv_comms.hh

@@ -69,7 +69,5 @@ class CommsClass {
   static void CommsTask(void* arg);
 };
 
-extern CommsClass Comms;
-
 }  // namespace comms
 }  // namespace ugv

main/ugv_io.cc

@@ -88,7 +88,7 @@ void IOClass::WriteOutputs(const Outputs &outputs) {
   ERROR_CHECK(ret);
   ret = gpio_set_level(MOTOR_RIGHT_DIR, right_dir);
   ERROR_CHECK(ret);
-  ESP_LOGD(TAG, "motor outputs: (%f, %f)", outputs.left_motor,
+  ESP_LOGV(TAG, "motor outputs: (%f, %f)", outputs.left_motor,
            outputs.right_motor);
 }
 

main/ugv_io.hh

@@ -35,7 +35,5 @@ class IOClass {
   void InitMotors();
 };
 
-extern IOClass IO;
-
 }  // namespace io
 }  // namespace ugv

main/ugv_io_gps.cc

@@ -73,7 +73,7 @@ void UART_GPS::Init() {
   ESP_LOGI(TAG, "gps uart configured");
 
   BaseType_t xRet = xTaskCreate(UART_GPS::GPS_Task, "ugv_io_gps", 2 * 1024,
-                                this, 1, &this->task_);
+                                this, 3, &this->task_);
   if (xRet != pdTRUE) {
     ESP_LOGE(TAG, "error creating GPS task");
     return;

main/ugv_main.cc

@@ -1,31 +1,69 @@
 #include <esp_log.h>
-#include <string.h>
+#include <esp_timer.h>
-#include <u8g2.h>
-
 #include "ugv_comms.hh"
 #include "ugv_display.hh"
 #include "ugv_io.hh"
 
 namespace ugv {
 
-using ugv::comms::Comms;
+using ugv::comms::CommsClass;
-using ugv::io::IO;
+using ugv::io::IOClass;
-
-DisplayClass *display;
 
 static const char *TAG = "ugv_main";
+constexpr uint64_t LOOP_PERIOD_US = 1e6 / 1;
+
+extern "C" void OnTimeout(void *arg);
+
+struct State {
+ public:
+  CommsClass *       comms;
+  IOClass *          io;
+  DisplayClass *     display;
+  esp_timer_handle_t timer_handle;
+  io::Inputs         inputs;
+  io::Outputs        outputs;
+
+  State() {
+    comms   = new CommsClass();
+    io      = new IOClass();
+    display = new DisplayClass(comms, io);
+  }
+
+  void Init() {
+    comms->Init();
+    io->Init();
+    display->Init();
 
-void setup(void) {
+    esp_timer_init();
-  ESP_LOGI(TAG, "setup");
 
-  Comms.Init();
+    esp_timer_create_args_t timer_args;
-  IO.Init();
+    timer_args.callback = OnTimeout;
-  display = new DisplayClass(&Comms, &IO);
+    timer_args.arg = this;
-  display->Init();
+    timer_args.dispatch_method = ESP_TIMER_TASK;
+    timer_args.name = "ugv_main_loop";
+    esp_timer_create(&timer_args, &this->timer_handle);
+    esp_timer_start_periodic(timer_handle, LOOP_PERIOD_US);
+  }
+
+  void OnTick() {
+    ESP_LOGD(TAG, "OnTick");
+  }
+};
+
+extern "C" void OnTimeout(void *arg) {
+  State *state = (State*)arg;
+  state->OnTick();
 }
 
-}  // namespace ugv
+State *state;
 
-extern "C" void app_main() {
+void Setup(void) {
-  ugv::setup();
+  ESP_LOGI(TAG, "Starting UAS UGV");
+  state = new State();
+  state->Init();
+  ESP_LOGI(TAG, "Setup finished");
 }
+
+}  // namespace ugv
+
+extern "C" void app_main() { ugv::Setup(); }