simulator_mavlink: Add ESC telemetry

src/modules/simulator/simulator.h

@@ -56,9 +56,12 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/input_rc.h>
+#include <uORB/topics/esc_status.h>
+#include <uORB/topics/esc_report.h>
 #include <uORB/topics/irlock_report.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/optical_flow.h>
@@ -190,6 +193,7 @@ private:
 	uORB::Publication<differential_pressure_s>	_differential_pressure_pub{ORB_ID(differential_pressure)};
 	uORB::PublicationMulti<optical_flow_s>		_flow_pub{ORB_ID(optical_flow)};
 	uORB::Publication<irlock_report_s>		_irlock_report_pub{ORB_ID(irlock_report)};
+	uORB::Publication<esc_status_s>			_esc_status_pub{ORB_ID(esc_status)};
 	uORB::Publication<vehicle_odometry_s>		_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
 	uORB::Publication<vehicle_odometry_s>		_mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
 
@@ -232,6 +236,7 @@ private:
 	void send();
 	void send_controls();
 	void send_heartbeat();
+	void send_esc_telemetry(mavlink_hil_actuator_controls_t hil_act_control);
 	void send_mavlink_message(const mavlink_message_t &aMsg);
 	void update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &sensors);
 
@@ -259,12 +264,14 @@ private:
 
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
 
 	// hil map_ref data
 	MapProjection _global_local_proj_ref{};
 	float _global_local_alt0{NAN};
 
 	vehicle_status_s _vehicle_status{};
+	battery_status_s _battery_status{};
 
 	bool _accel_blocked[ACCEL_COUNT_MAX] {};
 	bool _accel_stuck[ACCEL_COUNT_MAX] {};
@@ -293,6 +300,8 @@ private:
 	float _last_baro_pressure{0.0f};
 	float _last_baro_temperature{0.0f};
 
+	int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
+
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 	px4::atomic<bool> _has_initialized {false};
 #endif

src/modules/simulator/simulator_mavlink.cpp

@@ -87,6 +87,12 @@ Simulator::Simulator()
 	int32_t sys_ctrl_alloc = 0;
 	param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc);
 	_use_dynamic_mixing = sys_ctrl_alloc >= 1;
+
+	for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; ++i) {
+		char param_name[17];
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", "PWM_MAIN", "FUNC", i + 1);
+		param_get(param_find(param_name), &_output_functions[i]);
+	}
 }
 
 void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *msg)
@@ -200,6 +206,29 @@ void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *
 #endif
 }
 
+void Simulator::send_esc_telemetry(mavlink_hil_actuator_controls_t hil_act_control)
+{
+	esc_status_s esc_status{};
+	esc_status.timestamp = hrt_absolute_time();
+	esc_status.esc_count = math::min(actuator_outputs_s::NUM_ACTUATOR_OUTPUTS, esc_status_s::CONNECTED_ESC_MAX);
+
+	const bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+	esc_status.esc_armed_flags = armed ? 255 : 0;  // ugly
+
+	for (int i = 0; i < esc_status.esc_count; i++) {
+		esc_status.esc[i].actuator_function = _output_functions[i]; // TODO: this should be in pwm_out_sim...
+		esc_status.esc[i].timestamp = esc_status.timestamp;
+		esc_status.esc[i].esc_errorcount = 0; // TODO
+		esc_status.esc[i].esc_voltage = _battery_status.voltage_v;
+		esc_status.esc[i].esc_current = armed ? 1.0 + math::abs_t(hil_act_control.controls[i]) * 15.0 :
+						0.0f; // TODO: magic number
+		esc_status.esc[i].esc_rpm = hil_act_control.controls[i] * 6000;  // TODO: magic number
+		esc_status.esc[i].esc_temperature = 20.0 + math::abs_t(hil_act_control.controls[i]) * 40.0;
+	}
+
+	_esc_status_pub.publish(esc_status);
+}
+
 void Simulator::send_controls()
 {
 	orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs);
@@ -214,6 +243,8 @@ void Simulator::send_controls()
 		PX4_DEBUG("sending controls t=%ld (%ld)", _actuator_outputs.timestamp, hil_act_control.time_usec);
 
 		send_mavlink_message(message);
+
+		send_esc_telemetry(hil_act_control);
 	}
 }
 
@@ -756,6 +787,7 @@ void Simulator::send()
 			parameters_update(false);
 			check_failure_injections();
 			_vehicle_status_sub.update(&_vehicle_status);
+			_battery_status_sub.update(&_battery_status);
 
 			// Wait for other modules, such as logger or ekf2
 			px4_lockstep_wait_for_components();