HITL/SIH: Battery status depend on battery configuration from parameters

src/modules/mavlink/mavlink_receiver.cpp

@@ -2432,19 +2432,32 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 	}
 
 	// battery status
-	{
-		battery_status_s hil_battery_status{};
+	publish_hil_battery();
+}
+
+void
+MavlinkReceiver::publish_hil_battery()
+{
+	battery_status_s hil_battery_status{};
 
-		hil_battery_status.timestamp = timestamp;
-		hil_battery_status.voltage_v = 16.0f;
-		hil_battery_status.current_a = 10.0f;
-		hil_battery_status.discharged_mah = -1.0f;
-		hil_battery_status.connected = true;
-		hil_battery_status.remaining = 0.70;
-		hil_battery_status.time_remaining_s = NAN;
+	hil_battery_status.timestamp = hrt_absolute_time();
 
-		_battery_pub.publish(hil_battery_status);
+	hil_battery_status.cell_count = _param_bat_cells_count.get();
+	hil_battery_status.remaining = 0.70f;
+	float cells_v = _param_bat_v_empty.get() * (1.f - hil_battery_status.remaining)
+			+ hil_battery_status.remaining * _param_bat_v_charged.get();
+
+	hil_battery_status.voltage_v = hil_battery_status.cell_count * cells_v;
+	hil_battery_status.current_a = 10.0f;
+	hil_battery_status.discharged_mah = -1.0f;
+	hil_battery_status.connected = true;
+	hil_battery_status.time_remaining_s = NAN;
+
+	for (auto cell_count = 0; cell_count < hil_battery_status.cell_count; cell_count++) {
+		hil_battery_status.voltage_cell_v[cell_count] = cells_v;
 	}
+
+	_battery_pub.publish(hil_battery_status);
 }
 
 void
@@ -2767,15 +2780,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 	}
 
 	/* battery status */
-	{
-		battery_status_s hil_battery_status{};
-		hil_battery_status.voltage_v = 11.1f;
-		hil_battery_status.current_a = 10.0f;
-		hil_battery_status.discharged_mah = -1.0f;
-		hil_battery_status.timestamp = hrt_absolute_time();
-		hil_battery_status.time_remaining_s = NAN;
-		_battery_pub.publish(hil_battery_status);
-	}
+	publish_hil_battery();
 }
 
 #if !defined(CONSTRAINED_FLASH)

src/modules/mavlink/mavlink_receiver.h

@@ -244,6 +244,8 @@ class MavlinkReceiver : public ModuleParams
 	void update_message_statistics(const mavlink_message_t &message);
 	void update_rx_stats(const mavlink_message_t &message);
 
+	void publish_hil_battery();
+
 	px4::atomic_bool 	_should_exit{false};
 	pthread_t		_thread {};
 	/**
@@ -409,7 +411,10 @@ class MavlinkReceiver : public ModuleParams
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::BAT_CRIT_THR>)     _param_bat_crit_thr,
 		(ParamFloat<px4::params::BAT_EMERGEN_THR>)  _param_bat_emergen_thr,
-		(ParamFloat<px4::params::BAT_LOW_THR>)      _param_bat_low_thr
+		(ParamFloat<px4::params::BAT_LOW_THR>)      _param_bat_low_thr,
+		(ParamInt<px4::params::BAT1_N_CELLS>)       _param_bat_cells_count,
+		(ParamFloat<px4::params::BAT1_V_CHARGED>)   _param_bat_v_charged,
+		(ParamFloat<px4::params::BAT1_V_EMPTY>)     _param_bat_v_empty
 	);
 
 	// Disallow copy construction and move assignment.