Drone.hpp

#pragma once
#include <eventpp/callbacklist.h>
 
#include <memory>
#include <optional>
#include <string>
 
#include "DroneError.hpp"
#include "imu/ThreadedMPU9250.hpp"
#include "logging/Logger.hpp"
#include "observer/PioneerObserver.hpp"
#include "pmu/NonBlockingPMU.hpp"
#include "pmu/TcpPmu.hpp"
#include "sensors/FactoryPressureSensor.hpp"
#include "sensors/PressureSensor.hpp"
#include "thrust_allocation/ControlForce.hpp"
#include "thrust_allocation/PioneerAllocator.hpp"
#include "tilt/ITiltControl.hpp"
 
namespace blunux::drone {
 
class Drone : public logging::Logger<Drone> {
 public:
  explicit Drone(std::array<std::shared_ptr<imu::IImu>, 2> imu,
                 std::shared_ptr<pmu::IPMU> pmu,
                 std::shared_ptr<tilt::ITiltControl> tilt_controller);
 
  void set_light(float intensity);
 
  void set_indicator(blunux::pmu::IndicatorsSetpoint indicators);
 
  float get_light() const;
 
  float get_depth(bool wait_for_data = true) const;
 
  float get_water_temperature(bool wait_for_data = true) const;
 
  std::string get_blunux_version() const;
 
  std::string get_model_name() const;
 
  blueye::protocol::Model get_model() const;
 
  pmu::IndicatorsSetpoint get_indicator() const;
 
  imu::ImuData get_imu_raw(unsigned int n) const;
 
  imu::ImuData get_imu() const;
 
  imu::ImuSelfTest imu_self_test(unsigned int n) const;
 
  sensors::DepthSelfTestResult depth_self_test() const;
 
  std::optional<float> get_battery_percentage(bool wait_for_data = true) const;
  std::optional<std::string> get_battery_type(bool wait_for_data = true) const;
 
  void add_pressure_callback(std::function<void(const Pressure&)> cb);
  void add_temperature_callback(std::function<void(const Temperature&)> cb);
  void add_error_callback(std::function<void(const DroneError&)> cb);
 
  std::shared_ptr<tilt::ITiltControl> tilt() {
    return std::dynamic_pointer_cast<tilt::ITiltControl>(tilt_controller);
  }
 
  std::shared_ptr<observer::ICompassCalibration> compass_calibration() {
    return observer->compass_calibration();
  }
 
  std::shared_ptr<pmu::IPMUCallbacks> pmu_callbacks() {
    return std::dynamic_pointer_cast<pmu::IPMUCallbacks>(pmu);
  }
 
  std::shared_ptr<observer::IObserverState> observer_state() {
    return std::dynamic_pointer_cast<observer::IObserverState>(observer);
  }
 
  void set_fluid_density(double density) {
    if (optional_pressure_sensor.has_value()) {
      optional_pressure_sensor.value()->set_fluid_density(density);
    }
  }
 
  double get_fluid_density() const {
    return optional_pressure_sensor.has_value()
               ? optional_pressure_sensor.value()->get_fluid_density()
               : 1000;
  }
 
  blueye::protocol::PressureSensorType get_depth_sensor_type() const {
    return optional_pressure_sensor.has_value()
               ? optional_pressure_sensor.value()->get_sensor_type()
               : blueye::protocol::PRESSURE_SENSOR_TYPE_UNSPECIFIED;
  }
 
  void clear_callbacks();
 
 protected:
  const std::array<std::shared_ptr<imu::IImu>, 2> imu;
 
  const std::shared_ptr<pmu::IPMU> pmu;
 
  std::shared_ptr<observer::PioneerObserver> observer;
 
 private:
  std::optional<std::unique_ptr<sensors::PressureSensor>>
      optional_pressure_sensor =
          sensors::FactoryPressureSensor::create_sensor();
 
  // TODO(Johannes) lock
  std::optional<blunux::sensors::Pressure> pressure;
  std::optional<blunux::sensors::Temperature> temperature;
 
  // Eigen::Vector4d control_force;
 
  std::shared_ptr<tilt::ITiltControl> tilt_controller;
 
  eventpp::CallbackList<void(const Pressure&)> callbacks_pressure;
  eventpp::CallbackList<void(const Temperature&)> callbacks_temperature;
  eventpp::CallbackList<void(const DroneError&)> callbacks_error;
};
 
}  // namespace blunux::drone