diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e68353eabd..9c8e0099b0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2385,19 +2385,32 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) } // battery status - { - battery_status_s hil_battery_status{}; + publish_hil_battery(); +} - 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; +void +MavlinkReceiver::publish_hil_battery() +{ + battery_status_s hil_battery_status{}; - _battery_pub.publish(hil_battery_status); + hil_battery_status.timestamp = hrt_absolute_time(); + + 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 @@ -2720,15 +2733,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) diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 6c7b3065e2..d1c477e1c7 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -240,6 +240,8 @@ private: 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 {}; /** @@ -403,7 +405,10 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_bat_crit_thr, (ParamFloat) _param_bat_emergen_thr, - (ParamFloat) _param_bat_low_thr + (ParamFloat) _param_bat_low_thr, + (ParamInt) _param_bat_cells_count, + (ParamFloat) _param_bat_v_charged, + (ParamFloat) _param_bat_v_empty ); // Disallow copy construction and move assignment.