From 87815d886944a17ebd6f22d871c0c1254c48069a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 16 Sep 2022 14:42:17 -0400
Subject: [PATCH] simulator_ignition_bridge: add simple esc_status publication

 - ideally we'd get this from the actual joint velocity, but this is
good enough to start
---
 .../SimulatorIgnitionBridge.cpp               | 68 ++++++++++++++++---
 .../SimulatorIgnitionBridge.hpp               |  3 +
 2 files changed, 63 insertions(+), 8 deletions(-)

diff --git a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp
index dfa5a06342..129e6decec 100644
--- a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp
+++ b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp
@@ -139,7 +139,7 @@ int SimulatorIgnitionBridge::init()
 		return PX4_ERROR;
 	}
 
-	// IMU: /world/$WORLD/model/$MODEL//link/base_link/sensor/imu_sensor/imu
+	// IMU: /world/$WORLD/model/$MODEL/link/base_link/sensor/imu_sensor/imu
 	std::string imu_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/base_link/sensor/imu_sensor/imu";
 
 	if (!_node.Subscribe(imu_topic, &SimulatorIgnitionBridge::imuCallback, this)) {
@@ -147,12 +147,22 @@ int SimulatorIgnitionBridge::init()
 		return PX4_ERROR;
 	}
 
+	// ESC feedback: /model/X3/command/motor_speed
+	std::string motor_speed_topic = "/model/" + _model_name + "/command/motor_speed";
+
+	if (!_node.Subscribe(motor_speed_topic, &SimulatorIgnitionBridge::motorSpeedCallback, this)) {
+		PX4_ERR("failed to subscribe to %s", motor_speed_topic.c_str());
+		return PX4_ERROR;
+	}
+
+	// list all subscriptions
 	for (auto &sub_topic : _node.SubscribedTopics()) {
 		PX4_INFO("subscribed: %s", sub_topic.c_str());
 	}
 
-	// output eg /X3/command/motor_speed
-	std::string actuator_topic = "model/" + _model_name + "/command/motor_speed";
+
+	// output eg /model/X3/command/motor_speed
+	std::string actuator_topic = "/model/" + _model_name + "/command/motor_speed";
 	_actuators_pub = _node.Advertise<ignition::msgs::Actuators>(actuator_topic);
 
 	if (!_actuators_pub.Valid()) {
@@ -465,18 +475,60 @@ void SimulatorIgnitionBridge::poseInfoCallback(const ignition::msgs::Pose_V &pos
 	pthread_mutex_unlock(&_mutex);
 }
 
+void SimulatorIgnitionBridge::motorSpeedCallback(const ignition::msgs::Actuators &actuators)
+{
+	if (hrt_absolute_time() == 0) {
+		return;
+	}
+
+	pthread_mutex_lock(&_mutex);
+
+	esc_status_s esc_status{};
+	esc_status.esc_count = actuators.velocity_size();
+
+	for (int i = 0; i < actuators.velocity_size(); i++) {
+		esc_status.esc[i].timestamp = hrt_absolute_time();
+		esc_status.esc[i].esc_rpm = actuators.velocity(i);
+		esc_status.esc_online_flags |= 1 << i;
+
+		if (actuators.velocity(i) > 0) {
+			esc_status.esc_armed_flags |= 1 << i;
+		}
+	}
+
+	if (esc_status.esc_count > 0) {
+		esc_status.timestamp = hrt_absolute_time();
+		_esc_status_pub.publish(esc_status);
+	}
+
+	pthread_mutex_unlock(&_mutex);
+}
+
 bool SimulatorIgnitionBridge::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
 		unsigned num_control_groups_updated)
 {
-	ignition::msgs::Actuators rotor_velocity_message;
-	rotor_velocity_message.mutable_velocity()->Resize(num_outputs, 0);
+	unsigned active_output_count = 0;
 
 	for (unsigned i = 0; i < num_outputs; i++) {
-		rotor_velocity_message.set_velocity(i, outputs[i]);
+		if (_mixing_output.isFunctionSet(i)) {
+			active_output_count++;
+
+		} else {
+			break;
+		}
 	}
 
-	if (_actuators_pub.Valid()) {
-		return _actuators_pub.Publish(rotor_velocity_message);
+	if (active_output_count > 0) {
+		ignition::msgs::Actuators rotor_velocity_message;
+		rotor_velocity_message.mutable_velocity()->Resize(active_output_count, 0);
+
+		for (unsigned i = 0; i < active_output_count; i++) {
+			rotor_velocity_message.set_velocity(i, outputs[i]);
+		}
+
+		if (_actuators_pub.Valid()) {
+			return _actuators_pub.Publish(rotor_velocity_message);
+		}
 	}
 
 	return false;
diff --git a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp
index 69ca753641..bc3228ed03 100644
--- a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp
+++ b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp
@@ -44,6 +44,7 @@
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/esc_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_gyro.h>
@@ -93,10 +94,12 @@ private:
 	void clockCallback(const ignition::msgs::Clock &clock);
 	void imuCallback(const ignition::msgs::IMU &imu);
 	void poseInfoCallback(const ignition::msgs::Pose_V &pose);
+	void motorSpeedCallback(const ignition::msgs::Actuators &actuators);
 
 	// Subscriptions
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
+	uORB::Publication<esc_status_s>               _esc_status_pub{ORB_ID(esc_status)};
 	uORB::Publication<vehicle_angular_velocity_s> _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
 	uORB::Publication<vehicle_attitude_s>         _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
 	uORB::Publication<vehicle_global_position_s>  _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};