hil_interface.h

Go to the documentation of this file.

/*
 * Copyright 2016 Pavel Vechersky, ASL, ETH Zurich, Switzerland
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef ROTORS_HIL_INTERFACE_H_
#define ROTORS_HIL_INTERFACE_H_

#include <mav_msgs/Actuators.h>
#include <mav_msgs/default_topics.h>
#include <mavros_msgs/HilControls.h>
#include <mavros_msgs/mavlink_convert.h>

#ifndef MAVLINK_H
  typedef mavlink::mavlink_message_t mavlink_message_t;
  #include <mavlink/v2.0/common/mavlink.h>
#endif 

#include <rotors_hil_interface/hil_listeners.h>

namespace rotors_hil {
// Constants
static constexpr int kAllFieldsUpdated = 4095;

// Default values
static constexpr double kDefaultGpsFrequency = 5.0;
static const std::string kDefaultPressureSubTopic = "air_pressure";

inline u_int64_t RosTimeToMicroseconds(const ros::Time& rostime) {
  return (static_cast<uint64_t>(rostime.nsec * 1e-3) +
          static_cast<uint64_t>(rostime.sec * 1e6));
}

class HilInterface {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  virtual ~HilInterface() {};

  std::vector<mavros_msgs::Mavlink> virtual CollectData() = 0;

 protected:
  ros::NodeHandle nh_;

  ros::Subscriber air_speed_sub_;

  ros::Subscriber gps_sub_;

  ros::Subscriber ground_speed_sub_;

  ros::Subscriber imu_sub_;

  ros::Subscriber mag_sub_;

  ros::Subscriber pressure_sub_;

  Eigen::Quaterniond q_S_B_;

  Eigen::Matrix3f R_S_B_;

  HilData hil_data_;

  HilListeners hil_listeners_;

  boost::mutex mtx_;
};

class HilSensorLevelInterface : public HilInterface {
 public:
  HilSensorLevelInterface(const Eigen::Quaterniond& q_S_B);

  virtual ~HilSensorLevelInterface();

  std::vector<mavros_msgs::Mavlink> CollectData();

 private:
  mavlink_hil_gps_t hil_gps_msg_;

  mavlink_hil_sensor_t hil_sensor_msg_;

  uint64_t gps_interval_nsec_;

  uint64_t last_gps_pub_time_nsec_;
};

class HilStateLevelInterface : public HilInterface {
 public:
  HilStateLevelInterface(const Eigen::Quaterniond &q_S_B);

  virtual ~HilStateLevelInterface();

  std::vector<mavros_msgs::Mavlink> CollectData();

 private:
  mavlink_hil_state_quaternion_t hil_state_qtrn_msg_;
};
}

#endif // ROTORS_HIL_INTERFACE_H_