Pr external battery monitoring (#4881)

* mavlink receiver: added handling of battery status

handle incoming battery status messages in order to support external
battery monitoring

Signed-off-by: Roman <bapstr@ethz.ch>

* sensor params: added parameter for battery monitoring source

Signed-off-by: Roman <bapstr@ethz.ch>

* sensors: only publish battery status if we don't have external battery
monitoring activated

Signed-off-by: Roman <bapstr@ethz.ch>

src/modules/mavlink/mavlink_receiver.cpp

@@ -258,6 +258,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	case MAVLINK_MSG_ID_GPS_RTCM_DATA:
 		handle_message_gps_rtcm_data(msg);
 		break;
+	case MAVLINK_MSG_ID_BATTERY_STATUS:
+		handle_message_battery_status(msg);
+		break;
 
 	default:
 		break;
@@ -1154,6 +1157,44 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg)
+{
+	// external battery measurements
+	mavlink_battery_status_t battery_mavlink;
+	mavlink_msg_battery_status_decode(msg, &battery_mavlink);
+
+	battery_status_s battery_status = {};
+	battery_status.timestamp = hrt_absolute_time();
+
+	float voltage_sum = 0.0f;
+	uint8_t cell_count = 0;
+	for (unsigned i = 0; i < 10;i++) {
+		if (battery_mavlink.voltages[i] < UINT16_MAX) {
+			voltage_sum += (float)(battery_mavlink.voltages[i]) / 1000.0f;
+		} else {
+			cell_count = i;
+			break;
+		}
+	}
+
+	battery_status.voltage_v = voltage_sum;
+	battery_status.voltage_filtered_v  = voltage_sum;
+	battery_status.current_a = battery_status.current_filtered_a = (float)(battery_mavlink.current_battery) / 10000.0f;
+	battery_status.current_filtered_a = battery_status.current_a;
+	battery_status.remaining = (float)battery_mavlink.battery_remaining / 100.0f;
+	battery_status.discharged_mah = (float)battery_mavlink.current_consumed;
+	battery_status.cell_count = cell_count;
+	battery_status.connected = true;
+
+	if (_battery_pub == nullptr) {
+		_battery_pub = orb_advertise(ORB_ID(battery_status), &battery_status);
+
+	} else {
+		orb_publish(ORB_ID(battery_status), _battery_pub, &battery_status);
+	}
+}
+
 switch_pos_t
 MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw)
 {

src/modules/mavlink/mavlink_receiver.h

@@ -142,6 +142,7 @@ private:
 	void handle_message_follow_target(mavlink_message_t *msg);
 	void handle_message_adsb_vehicle(mavlink_message_t *msg);
 	void handle_message_gps_rtcm_data(mavlink_message_t *msg);
+	void handle_message_battery_status(mavlink_message_t *msg);
 
 	void *receive_thread(void *arg);
 

src/modules/sensors/sensor_params.c

@@ -2023,6 +2023,22 @@ PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0);
  */
 PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0);
 
+/**
+ * Battery monitoring source.
+ *
+ * This parameter controls the source of battery data. The value 'Power Module'
+ * means that measurements are expected to come from a power module. If the value is set to
+ * 'External' then the system expects to receive mavlink battery status messages.
+ *
+ * @min 0
+ * @max 1
+ * @value 0 Power Module
+ * @value 1 External
+ * @group Battery Calibration
+ */
+
+PARAM_DEFINE_INT32(BAT_SOURCE, 0);
+
 
 /**
  * RC channel count

src/modules/sensors/sensors.cpp

@@ -319,6 +319,7 @@ private:
 		float battery_current_offset;
 		float battery_v_div;
 		float battery_a_per_v;
+		int32_t battery_source;
 
 		float baro_qnh;
 
@@ -381,6 +382,7 @@ private:
 		param_t battery_current_offset;
 		param_t battery_v_div;
 		param_t battery_a_per_v;
+		param_t battery_source;
 
 		param_t board_rotation;
 
@@ -653,6 +655,7 @@ Sensors::Sensors() :
 	_parameter_handles.battery_current_offset = param_find("BAT_V_OFFS_CURR");
 	_parameter_handles.battery_v_div = param_find("BAT_V_DIV");
 	_parameter_handles.battery_a_per_v = param_find("BAT_A_PER_V");
+	_parameter_handles.battery_source = param_find("BAT_SOURCE");
 
 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
@@ -948,6 +951,8 @@ Sensors::parameters_update()
 #endif
 	}
 
+	param_get(_parameter_handles.battery_source, &(_parameters.battery_source));
+
 	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 
@@ -1718,7 +1723,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 				}
 			}
 
-			if (updated_battery) {
+			if (_parameters.battery_source == 0 && updated_battery) {
 				actuator_controls_s ctrl;
 				orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl);
 				_battery.updateBatteryStatus(t, bat_voltage_v, bat_current_a, ctrl.control[actuator_controls_s::INDEX_THROTTLE],