uas-ugv/main/ugv_main.cc

#include <esp_log.h>
#include <esp_timer.h>
#include "MadgwickAHRS.h"
#include "i2c_mutex.h"
#include "ugv_comms.hh"
#include "ugv_display.hh"
#include "ugv_io.hh"

#include <math.h>

namespace ugv {

using ugv::comms::CommsClass;
using ugv::io::IOClass;

static const char *TAG = "ugv_main";

extern "C" {
SemaphoreHandle_t i2c_mutex;
}

constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
static const float PI             = atanf(1.0) * 4.0;

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;
  int64_t            last_print;
  Madgwick           ahrs_;

  State() {
    comms   = new CommsClass();
    io      = new IOClass();
    display = new DisplayClass(comms);
  }

  void Init() {
    esp_timer_init();
    i2c_mutex = xSemaphoreCreateMutex();

    ahrs_.begin(1000000.f /
                static_cast<float>(LOOP_PERIOD_US));  // rough sample frequency

    comms->Init();
    display->Init();
    io->Init();

    esp_timer_create_args_t timer_args;
    timer_args.callback        = OnTimeout;
    timer_args.arg             = this;
    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);
    last_print = 0;
  }

  void OnTick() {
    ESP_LOGV(TAG, "OnTick");
    int64_t time_us = esp_timer_get_time();
    float   time_s  = ((float)time_us) / 1e6;
    io->ReadInputs(inputs);
    outputs.left_motor  = sinf(time_s * PI);
    outputs.right_motor = cosf(time_s * PI);
    io->WriteOutputs(outputs);
    {
      io::Vec3f &g = inputs.mpu.gyro_rate, &a = inputs.mpu.accel,
                &m = inputs.mpu.mag;
      ahrs_.update(g.x, g.y, g.z, a.x, a.y, a.z, m.x, m.y, m.z);
    }
    if (time_us >= last_print + 500 * 1000) {  // 1s
      ESP_LOGD(TAG,
               "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)",
               inputs.mpu.accel.x, inputs.mpu.accel.y, inputs.mpu.accel.z,
               inputs.mpu.gyro_rate.x, inputs.mpu.gyro_rate.y,
               inputs.mpu.gyro_rate.z, inputs.mpu.mag.x, inputs.mpu.mag.y,
               inputs.mpu.mag.z);
      ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(),
               ahrs_.getPitch(), ahrs_.getRoll());
      last_print = time_us;
    }
  }
};

extern "C" void OnTimeout(void *arg) {
  State *state = (State *)arg;
  state->OnTick();
}

State *state;

void Setup(void) {
  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(); }