Add sending esc_status messages to the drone

This commit is just fragments of code to study passing esc info from gazebo to drone in hitl.

Signed-off-by: Jukka Laitinen <[email protected]>

include/gazebo_mavlink_interface.h

@@ -143,6 +143,7 @@ namespace mavlink_interface
       void MagnetometerCallback(const gz::msgs::Magnetometer &_msg);
       void GpsCallback(const gz::msgs::NavSat &_msg);
       void SendSensorMessages(const gz::sim::UpdateInfo &_info);
+      void SendStatusMessages(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm);
       void PublishRotorVelocities(gz::sim::EntityComponentManager &_ecm,
           const Eigen::VectorXd &_vels);
       void handle_actuator_controls(const gz::sim::UpdateInfo &_info);

include/mavlink_interface.h

@@ -70,6 +70,8 @@ static constexpr auto kDefaultBaudRate = 921600;
 static constexpr ssize_t MAX_SIZE = MAVLINK_MAX_PACKET_LEN + 16;
 static constexpr size_t MAX_TXQ_SIZE = 1000;
 
+static constexpr ssize_t MAX_N_ESCS = 16;
+
 //! Rx packer framing status. (same as @p mavlink::mavlink_framing_t)
 enum class Framing : uint8_t {
 	incomplete = MAVLINK_FRAMING_INCOMPLETE,
@@ -110,6 +112,20 @@ namespace SensorData {
     };
 }
 
+namespace StatusData {
+    struct EscReport {
+        int rpm;
+	double voltage;
+        double current;
+    };
+
+    struct EscStatus {
+        int esc_count;
+        int esc_active_input;
+	struct EscReport esc[MAX_N_ESCS];
+    };
+}
+
 /*
 struct HILData {
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -143,6 +159,7 @@ class MavlinkInterface {
     void close();
     void Load();
     void SendSensorMessages(const uint64_t time_usec);
+    void SendEscStatusMessages(const uint64_t time_usec, struct StatusData::EscStatus &status);
     void UpdateBarometer(const SensorData::Barometer &data);
     void UpdateAirspeed(const SensorData::Airspeed &data);
     void UpdateIMU(const SensorData::Imu &data);

src/gazebo_mavlink_interface.cpp

@@ -26,10 +26,12 @@
 
 #include <gz/plugin/Register.hh>
 #include <gz/sensors/Sensor.hh>
+#include <gz/sim/Joint.hh>
 #include <gz/sim/components/AirPressureSensor.hh>
 #include <gz/sim/components/Magnetometer.hh>
 #include <gz/sim/components/Imu.hh>
 #include <gz/sim/components/Pose.hh>
+#include <gz/sim/components/Joint.hh>
 
 GZ_ADD_PLUGIN(
     mavlink_interface::GazeboMavlinkInterface,
@@ -255,6 +257,8 @@ void GazeboMavlinkInterface::PreUpdate(const gz::sim::UpdateInfo &_info,
   if (received_first_actuator_) {
     PublishRotorVelocities(_ecm, input_reference_);
   }
+
+  SendStatusMessages(_info, _ecm);
 }
 
 void GazeboMavlinkInterface::PostUpdate(const gz::sim::UpdateInfo &_info,
@@ -424,6 +428,65 @@ void GazeboMavlinkInterface::SendSensorMessages(const gz::sim::UpdateInfo &_info
   mavlink_interface_->SendSensorMessages(time_usec);
 }
 
+void GazeboMavlinkInterface::SendStatusMessages(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm) {
+  uint64_t time_usec = std::chrono::duration_cast<std::chrono::duration<uint64_t>>(_info.simTime * 1e6).count();
+  struct StatusData::EscStatus status;
+
+  // TOOD: find out how to properly get the RPM for each motor from the motor model. Multicopter motor model probably
+  // doesn't support that. The joint velocity (some draft code left below) might be only for visuals.
+  // There is additionally the scaler "rotorVelocitySlowdownSim" that should be taken into account if
+  // using the joint velocity.
+
+  // Buth actually both below are wrong; the motor constant doesn't affect the
+  // visual rotation speed in gazebo, it is only used for thrust calculation.
+
+#if 1
+
+  // Read the joint velocities from gazebo
+  // Note: CW values are positive, CCW negative
+
+  std::vector<double> vels;
+  char joint_name_c[] = "rotor_0_joint";
+  gz::sim::Entity jointEntity = _ecm.EntityByComponents(gz::sim::components::Name(joint_name_c), gz::sim::components::Joint());
+
+  double vel;
+  int i = 0;
+  while (jointEntity != gz::sim::kNullEntity) {
+    gz::sim::Joint joint(jointEntity);
+
+    std::optional<std::vector<double>> jointVelocity = joint.Velocity(_ecm);
+    if (jointVelocity && (*jointVelocity).size() > 0) {
+      vel = (*jointVelocity)[0]
+        / 1000 // maxRotVelocity
+        * 10;  // rotorVelocitySlowdownSim
+
+      vels.push_back(vel);
+    }
+
+    i++;
+    joint_name_c[6] = '0' + i;
+    jointEntity = _ecm.EntityByComponents(gz::sim::components::Name(joint_name_c), gz::sim::components::Joint());
+  }
+#else
+
+  // Just use the actuator controls directly
+  // Note: this bypasses the motor model entirely and "fakes" the RPM.
+  // All RPM values are positive
+
+  auto vels = mavlink_interface_->GetActuatorControls();
+#endif
+
+  status.esc_count = vels.size();
+
+  for (int i = 0; i < vels.size(); i++) {
+    status.esc[i].rpm = vels[i]
+      / 0.00000854858 // motorConstant
+      / (2 * 3.14);   // rad/s -> RPM
+  }
+
+  mavlink_interface_->SendEscStatusMessages(time_usec, status);
+}
+
 void GazeboMavlinkInterface::handle_actuator_controls(const gz::sim::UpdateInfo &_info) {
   bool armed = mavlink_interface_->GetArmedState();
 
@@ -557,4 +620,4 @@ void GazeboMavlinkInterface::RotateQuaternion(gz::math::Quaterniond &q_FRD_to_NE
 
 	// final rotation composition
 	q_FRD_to_NED = q_ENU_to_NED * q_FLU_to_ENU * q_FLU_to_FRD.Inverse();
-}
+}

src/mavlink_interface.cpp

@@ -417,6 +417,36 @@ void MavlinkInterface::SendSensorMessages(uint64_t time_usec) {
   PushSendMessage(msg_shared);
 }
 
+void MavlinkInterface::SendEscStatusMessages(uint64_t time_usec, struct StatusData::EscStatus &status) {
+  mavlink_esc_status_t esc_status_msg{};
+  mavlink_esc_info_t esc_info_msg{};
+  static constexpr uint8_t batch_size = MAVLINK_MSG_ESC_STATUS_FIELD_RPM_LEN;
+  static uint16_t counter = 0;
+
+  esc_status_msg.time_usec = time_usec;
+  for (int i = 0; i < batch_size; i++) {
+    esc_status_msg.rpm[i] = status.esc[i].rpm;
+
+    esc_info_msg.failure_flags[i] = 0;
+    esc_info_msg.error_count[i] = 0;
+    esc_info_msg.temperature[i] = 20;
+  }
+
+  mavlink_message_t msg;
+  mavlink_msg_esc_status_encode_chan(254, 25, MAVLINK_COMM_0, &msg, &esc_status_msg);
+  auto msg_shared = std::make_shared<mavlink_message_t>(msg);
+  PushSendMessage(msg_shared);
+
+  esc_info_msg.counter = counter++;
+  esc_info_msg.count = status.esc_count;
+  esc_info_msg.connection_type = 0; // TODO: use two highest bits for selected input
+  esc_info_msg.info = (1u << status.esc_count) - 1;
+
+  mavlink_msg_esc_info_encode_chan(254, 25, MAVLINK_COMM_0, &msg, &esc_info_msg);
+  msg_shared = std::make_shared<mavlink_message_t>(msg);
+  PushSendMessage(msg_shared);
+}
+
 void MavlinkInterface::UpdateBarometer(const SensorData::Barometer &data) {
   const std::lock_guard<std::mutex> lock(sensor_msg_mutex_);
   temperature_ = data.temperature;