uas-ugv/main/ugv.hh

#pragma once

#include <esp_log.h>
#include <esp_timer.h>

#include "MadgwickAHRS.h"
#include "config.pb.h"
#include "lat_long.hh"
#include "pid_controller.hh"
#include "ugv_io.hh"

namespace ugv {

namespace comms {
class CommsClass;
}

class DisplayClass;

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

extern "C" void UGV_TickTimeout(void *arg);

void UpdateLocationFromGPS(comms::messages::Location &location,
                           const io::GpsData &        gps_data);

class UGV {
 private:
  CommsClass *       comms;
  IOClass *          io;
  DisplayClass *     display;
  esp_timer_handle_t timer_handle;

  LatLong        target_;
  config::Config conf_;
  PIDController  angle_controller_;
  Madgwick       ahrs_;

  io::Inputs  inputs_;
  io::Outputs outputs_;
  int64_t     last_print_;
  UGV_State   current_state_;
  UGV_State   next_state_;
  float       yaw_;
  float       pitch_;
  float       roll_;

  void UpdateAHRS();
  void DoDebugPrint();
  void ReadComms();

 public:
  explicit UGV();

  static config::Config DefaultConfig();

  void SetConfig(const config::Config &conf);
  void SetTarget(LatLong target);

  void Init();

  void OnTick();
};

extern UGV *the_ugv;

void Start(void);

}  // namespace ugv