[WIP] hacking around, don't use this

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,50 @@ 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;
+
+#if 0
+  const std::string jointName = "rotor_0_joint";
+  gz::sim::Entity jointEntity = _ecm.EntityByComponents(gz::sim::components::Name(jointName), gz::sim::components::Joint());
+
+  double vel;
+
+  if (jointEntity != gz::sim::kNullEntity) {
+	  gz::sim::Joint joint(jointEntity);
+
+	  std::optional<std::vector<double>> jointVelocity = joint.Velocity(_ecm);
+	  if (jointVelocity) {
+		  std::vector<double> vels = *jointVelocity;
+		  if (vels.size() > 0) {
+			  vel = vels[0];
+			  //std::cout << "Joint Velocity: " << vels[0] << std::endl;
+		  }
+	  }
+  }
+#endif
+
+  auto actuatorMsgComp =
+      _ecm.Component<gz::sim::components::Actuators>(model_.Entity());
+
+  if (actuatorMsgComp)
+  {
+	  gz::msgs::Actuators actuatorMsg = actuatorMsgComp->Data();
+	  actuatorMsg.velocity();
+  }
+
+  /*values should be read from the motor model? */
+  auto vels = mavlink_interface_->GetActuatorControls();
+  status.esc_count = vels.size();
+
+  for (int i = 0; i < vels.size(); i++) {
+	  status.esc[i].rpm = vel;
+  }
+
+  mavlink_interface_->SendEscStatusMessages(time_usec, status);
+}
+
 void GazeboMavlinkInterface::handle_actuator_controls(const gz::sim::UpdateInfo &_info) {
   bool armed = mavlink_interface_->GetArmedState();
 
@@ -557,4 +605,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,39 @@ 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;