uas-ugv/main/ugv_io.cc

#include "ugv_io.hh"

#include <driver/gpio.h>
#include <driver/mcpwm.h>
#include <driver/uart.h>
#include <esp_log.h>
#include <math.h>

#include "MPU.hpp"
#include "mpu/math.hpp"

namespace ugv {
namespace io {

static constexpr mcpwm_unit_t     MCPWM_UNIT      = MCPWM_UNIT_0;
static constexpr gpio_num_t       MOTOR_LEFT_PWM  = GPIO_NUM_0;
static constexpr gpio_num_t       MOTOR_LEFT_DIR  = GPIO_NUM_0;
static constexpr gpio_num_t       MOTOR_RIGHT_PWM = GPIO_NUM_0;
static constexpr gpio_num_t       MOTOR_RIGHT_DIR = GPIO_NUM_0;
static constexpr mpud::accel_fs_t MPU_ACCEL_FS    = mpud::ACCEL_FS_2G;
static constexpr mpud::gyro_fs_t  MPU_GYRO_FS     = mpud::GYRO_FS_500DPS;
static constexpr float            MPU_MAG_TO_FLUX = (4912.f) / (32760.f);

static constexpr size_t      GPS_BUF_SIZE         = 1024;
static constexpr uart_port_t GPS_UART             = UART_NUM_1;
static constexpr int         GPS_UART_TX_PIN      = 17;
static constexpr int         GPS_UART_RX_PIN      = 23;
static constexpr int         GPS_UART_BAUD        = 9600;
static constexpr size_t      GPS_UART_RX_BUF_SIZE = 1024;
static constexpr size_t      GPS_UART_TX_BUF_SIZE = 0;
static constexpr size_t      GPS_UART_QUEUE_SIZE  = 8;

const char NMEA_OUTPUT_RMCGGA[] =
    "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n";
const char NMEA_SET_UPDATE_1HZ[] = "$PMTK220,1000*1F\r\n";
const char NMEA_Q_RELEASE[]      = "$PMTK605*31\r\n";

static const char *TAG = "ugv_io";

IOClass IO;

IOClass::IOClass() { mpu_ = new mpud::MPU(); gps_ = new UART_GPS(); }

IOClass::~IOClass() { delete mpu_; delete gps_; }

void IOClass::Init() {
  InitMotors();
  InitMPU();
  gps_->Init();
}

void IOClass::InitMotors() {
  gpio_set_direction(MOTOR_LEFT_PWM, GPIO_MODE_OUTPUT);
  gpio_set_direction(MOTOR_LEFT_DIR, GPIO_MODE_OUTPUT);
  gpio_set_direction(MOTOR_RIGHT_PWM, GPIO_MODE_OUTPUT);
  gpio_set_direction(MOTOR_RIGHT_DIR, GPIO_MODE_OUTPUT);
  mcpwm_gpio_init(MCPWM_UNIT, MCPWM0A, MOTOR_LEFT_PWM);
  mcpwm_gpio_init(MCPWM_UNIT, MCPWM0B, MOTOR_RIGHT_PWM);

  mcpwm_config_t mcpwm_config;
  mcpwm_config.frequency    = 20000;  // 20khz
  mcpwm_config.cmpr_a       = 0;
  mcpwm_config.cmpr_b       = 0;
  mcpwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER;  // for symmetric pwm
  mcpwm_config.duty_mode    = MCPWM_DUTY_MODE_0;      // active high
  mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config);
}

void IOClass::InitMPU() {
  esp_err_t ret;
  ret = mpu_->testConnection();
  if (ret != ESP_OK) {
    ESP_LOGE(TAG, "MPU not connected");
    return;
  }
  ret = mpu_->initialize();
  if (ret != ESP_OK) {
    ESP_LOGE(TAG, "MPU initialization error");
    return;
  }
  mpu_->setAccelFullScale(MPU_ACCEL_FS);
  mpu_->setGyroFullScale(MPU_GYRO_FS);
  ESP_LOGI(TAG, "MPU initialized");
}

void IOClass::ReadInputs(Inputs &inputs) {
  esp_err_t ret = mpu_->motion(&accel_, &gyro_, &mag_);
  if (ret != ESP_OK) {
    ESP_LOGE(TAG, "error reading MPU");
  }
  gps_->GetInfo(inputs.gps_info);
  inputs.accel     = mpud::accelGravity(accel_, MPU_ACCEL_FS);
  inputs.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
  inputs.mag.x     = ((float)mag_.x) * MPU_MAG_TO_FLUX;
  inputs.mag.y     = ((float)mag_.y) * MPU_MAG_TO_FLUX;
  inputs.mag.z     = ((float)mag_.z) * MPU_MAG_TO_FLUX;
}

void IOClass::WriteOutputs(const Outputs &outputs) {
  mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A,
                 fabs(outputs.left_motor));
  mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_B,
                 fabs(outputs.right_motor));
  bool left_dir, right_dir;
  left_dir  = outputs.left_motor < 0.f;
  right_dir = outputs.right_motor < 0.f;
  gpio_set_level(MOTOR_LEFT_DIR, left_dir);
  gpio_set_level(MOTOR_RIGHT_DIR, right_dir);
}

};  // namespace io
};  // namespace ugv