From ba8eb4745142d8eb8ab44fbde987945dfcbfb3b9 Mon Sep 17 00:00:00 2001
From: Jaeyoung-Lim <jalim@ethz.ch>
Date: Mon, 27 Sep 2021 23:52:19 +0200
Subject: [PATCH] Handle battery status messages Handle simulator mavlink
 messages Fix

---
 src/modules/simulator/simulator.h           |  3 +++
 src/modules/simulator/simulator_mavlink.cpp | 19 +++++++++++++++++++
 2 files changed, 22 insertions(+)

diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 5e90ad7bab..346f902290 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -58,6 +58,7 @@
 #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/irlock_report.h>
@@ -228,6 +229,7 @@ private:
 	void handle_message_optical_flow(const mavlink_message_t *msg);
 	void handle_message_rc_channels(const mavlink_message_t *msg);
 	void handle_message_vision_position_estimate(const mavlink_message_t *msg);
+	void handle_message_battery_status(const mavlink_message_t *msg);
 
 	void parameters_update(bool force);
 	void poll_for_MAVLink_messages();
@@ -249,6 +251,7 @@ private:
 	uORB::Publication<vehicle_global_position_s>	_gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};
 	uORB::Publication<vehicle_local_position_s>	_lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)};
 	uORB::Publication<input_rc_s>			_input_rc_pub{ORB_ID(input_rc)};
+	uORB::Publication<battery_status_s> 		_battery_pub{ORB_ID(battery_status)};
 
 	// HIL GPS
 	static constexpr int MAX_GPS = 3;
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index dad520015e..8b7b7a55ee 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -368,6 +368,11 @@ void Simulator::handle_message(const mavlink_message_t *msg)
 	case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
 		handle_message_hil_state_quaternion(msg);
 		break;
+
+	case MAVLINK_MSG_ID_BATTERY_STATUS:
+		handle_message_battery_status(msg);
+		break;
+
 	}
 }
 
@@ -672,6 +677,20 @@ void Simulator::send_mavlink_message(const mavlink_message_t &aMsg)
 	}
 }
 
+void Simulator::handle_message_battery_status(const mavlink_message_t *msg)
+{
+	mavlink_battery_status_t battery_status_mavlink;
+	mavlink_msg_battery_status_decode(msg, &battery_status_mavlink);
+
+	battery_status_s battery_status{};
+	battery_status.voltage_v = 14.8f;
+	battery_status.voltage_filtered_v = 14.8f;
+	battery_status.current_a = 15.0f;
+	battery_status.discharged_mah = -1.0f;
+	battery_status.timestamp = hrt_absolute_time();
+	_battery_pub.publish(battery_status);
+}
+
 void *Simulator::sending_trampoline(void * /*unused*/)
 {
 	_instance->send();