battery: check source param inside battery lib

This moves the handling of the BAT%d_SOURCE param inside of the battery
library. Users of the library now pass the source instead of the flag
whether to publish. The battery library then checks if the source is
selected using the param and publishes accordingly.

Since we removed the strange system_source flag, we now need to look at
all batteries in commander.
For current estimation - I think - it makes sense to sum them up.

boards/bitcraze/crazyflie/syslink/syslink_main.cpp

@@ -411,7 +411,8 @@ Syslink::handle_message(syslink_message_t *msg)
 		memcpy(&vbat, &msg->data[1], sizeof(float));
 		//memcpy(&iset, &msg->data[5], sizeof(float));
 
-		_battery.updateBatteryStatus(t, vbat, -1, true, true, 0, 0, true);
+		_battery.updateBatteryStatus(t, vbat, -1, true,
+					     battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0);
 
 
 		// Update battery charge state

msg/battery_status.msg

@@ -10,7 +10,11 @@ float32 scale		# Power scaling factor, >= 1, or -1 if unknown
 float32 temperature         # temperature of the battery. NaN if unknown
 int32 cell_count		# Number of cells
 bool connected			# Whether or not a battery is connected, based on a voltage threshold
-bool system_source		# Whether or not a this battery is the active power source for VDD_5V_IN
+
+uint8 BATTERY_SOURCE_POWER_MODULE = 0
+uint8 BATTERY_SOURCE_EXTERNAL = 0
+uint8 BATTERY_SOURCE_ESCS = 0
+uint8 source		# Battery source
 uint8 priority        	# Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1
 uint16 capacity         # actual capacity of the battery
 uint16 cycle_count         # number of discharge cycles the battery has experienced

src/drivers/power_monitor/ina226/ina226.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -88,10 +88,9 @@ INA226::INA226(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int
 		0.0,
 		0.0,
 		false,
-		false, // TODO: selected source?
+		battery_status_s::BATTERY_SOURCE_POWER_MODULE,
 		0,
-		0.0,
-		true
+		0.0
 	);
 }
 
@@ -262,10 +261,9 @@ INA226::collect()
 			(float) _bus_voltage * INA226_VSCALE,
 			(float) _current * _current_lsb,
 			true,
-			true, // TODO: Determine if this is the selected source
+			battery_status_s::BATTERY_SOURCE_POWER_MODULE,
 			0,
-			_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE],
-			true
+			_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]
 		);
 
 		ret = OK;
@@ -276,10 +274,9 @@ INA226::collect()
 			0.0,
 			0.0,
 			false,
-			false, // TODO: selected source?
+			battery_status_s::BATTERY_SOURCE_POWER_MODULE,
 			0,
-			0.0,
-			true
+			0.0
 		);
 		ret = -1;
 		perf_count(_comms_errors);
@@ -352,10 +349,9 @@ INA226::RunImpl()
 			0.0f,
 			0.0f,
 			false,
-			false,
+			battery_status_s::BATTERY_SOURCE_POWER_MODULE,
 			0,
-			0.0f,
-			true
+			0.0f
 		);
 
 		if (init() != OK) {

src/drivers/power_monitor/voxlpm/voxlpm.cpp

@@ -149,7 +149,8 @@ VOXLPM::measure()
 
 		switch (_ch_type) {
 		case VOXLPM_CH_TYPE_VBATT: {
-				_battery.updateBatteryStatus(tnow, _voltage, _amperage, true, true, 0, 0, true);
+				_battery.updateBatteryStatus(tnow, _voltage, _amperage, true,
+							     battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0);
 			}
 
 		// fallthrough
@@ -170,7 +171,8 @@ VOXLPM::measure()
 	} else {
 		switch (_ch_type) {
 		case VOXLPM_CH_TYPE_VBATT: {
-				_battery.updateBatteryStatus(tnow, 0.0, 0.0, true, true, 0, 0, true);
+				_battery.updateBatteryStatus(tnow, 0.0, 0.0, true,
+							     battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0);
 			}
 			break;
 

src/drivers/uavcan/sensors/battery.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -95,7 +95,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 	// battery.average_time_to_empty = msg.;
 	battery.serial_number = msg.model_instance_id;
 	battery.connected = true;
-	battery.system_source = msg.status_flags & uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE;
+	battery.source = battery_status_s::BATTERY_SOURCE_POWER_MODULE;
 	// battery.priority = msg.;
 	// battery.is_powering_off = msg.;
 	// battery.warning = msg.;

src/lib/battery/battery.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -121,8 +121,8 @@ Battery::reset()
 
 void
 Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a,
-			     bool connected, bool selected_source, int priority,
-			     float throttle_normalized, bool should_publish)
+			     bool connected, int source, int priority,
+			     float throttle_normalized)
 {
 	reset();
 	_battery_status.timestamp = timestamp;
@@ -148,7 +148,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 		_battery_status.warning = _warning;
 		_battery_status.remaining = _remaining;
 		_battery_status.connected = connected;
-		_battery_status.system_source = selected_source;
+		_battery_status.source = source;
 		_battery_status.priority = priority;
 
 		static constexpr int uorb_max_cells = sizeof(_battery_status.voltage_cell_v) / sizeof(
@@ -162,6 +162,8 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 
 	_battery_status.timestamp = timestamp;
 
+	const bool should_publish = (source == _params.source);
+
 	if (should_publish) {
 		publish();
 	}

src/lib/battery/battery.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -87,21 +87,18 @@ public:
 	 */
 	float full_cell_voltage() { return _params.v_charged; }
 
-	int source() { return _params.source; }
-
 	/**
 	 * Update current battery status message.
 	 *
 	 * @param voltage_raw: Battery voltage, in Volts
 	 * @param current_raw: Battery current, in Amps
 	 * @param timestamp: Time at which the ADC was read (use hrt_absolute_time())
-	 * @param selected_source: This battery is on the brick that the selected source for selected_source
+	 * @param source: Source type in relation to BAT%d_SOURCE param.
 	 * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
 	 * @param throttle_normalized: Throttle of the vehicle, between 0 and 1
-	 * @param should_publish If True, this function published a battery_status uORB message.
 	 */
 	void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, bool connected,
-				 bool selected_source, int priority, float throttle_normalized, bool should_publish);
+				 int source, int priority, float throttle_normalized);
 
 	/**
 	 * Publishes the uORB battery_status message with the most recently-updated data.

src/modules/battery_status/analog_battery.cpp

@@ -41,7 +41,7 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent) :
 
 void
 AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw,
-				      bool selected_source, int priority, float throttle_normalized)
+				      int source, int priority, float throttle_normalized)
 {
 	float voltage_v = voltage_raw * _analog_params.v_div;
 	float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v;
@@ -51,7 +51,7 @@ AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw,
 
 
 	Battery::updateBatteryStatus(timestamp, voltage_v, current_a, connected,
-				     selected_source, priority, throttle_normalized, _params.source == 0);
+				     source, priority, throttle_normalized);
 }
 
 bool AnalogBattery::is_valid()

src/modules/battery_status/analog_battery.h

@@ -47,12 +47,12 @@ public:
 	 * @param voltage_raw Battery voltage read from ADC, volts
 	 * @param current_raw Voltage of current sense resistor, volts
 	 * @param timestamp Time at which the ADC was read (use hrt_absolute_time())
-	 * @param selected_source This battery is on the brick that the selected source for selected_source
+	 * @param source The source as defined by param BAT%d_SOURCE
 	 * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
 	 * @param throttle_normalized Throttle of the vehicle, between 0 and 1
 	 */
 	void updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw,
-				    bool selected_source, int priority, float throttle_normalized);
+				    int source, int priority, float throttle_normalized);
 
 	/**
 	 * Whether the ADC channel for the voltage of this battery is valid.

src/modules/battery_status/battery_status.cpp

@@ -176,11 +176,6 @@ BatteryStatus::adc_poll()
 	float bat_current_adc_readings[BOARD_NUMBER_BRICKS] {};
 	float bat_voltage_adc_readings[BOARD_NUMBER_BRICKS] {};
 
-	/* Based on the valid_chan, used to indicate the selected the lowest index
-	 * (highest priority) supply that is the source for the VDD_5V_IN
-	 * When < 0 none selected
-	 */
-
 	int selected_source = -1;
 
 	adc_report_s adc_report;
@@ -200,7 +195,6 @@ BatteryStatus::adc_poll()
 					 * VDD_5V_IN
 					 */
 					selected_source = b;
-
 				}
 
 				/* look for specific channels and process the raw voltage to measurement data */
@@ -221,19 +215,18 @@ BatteryStatus::adc_poll()
 		}
 
 		for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
-			if (_analogBatteries[b]->source() == 0) {
-				actuator_controls_s ctrl{};
-				_actuator_ctrl_0_sub.copy(&ctrl);
 
-				_analogBatteries[b]->updateBatteryStatusADC(
-					hrt_absolute_time(),
-					bat_voltage_adc_readings[b],
-					bat_current_adc_readings[b],
-					selected_source == b,
-					b,
-					ctrl.control[actuator_controls_s::INDEX_THROTTLE]
-				);
-			}
+			actuator_controls_s ctrl{};
+			_actuator_ctrl_0_sub.copy(&ctrl);
+
+			_analogBatteries[b]->updateBatteryStatusADC(
+				hrt_absolute_time(),
+				bat_voltage_adc_readings[b],
+				bat_current_adc_readings[b],
+				battery_status_s::BATTERY_SOURCE_POWER_MODULE,
+				b,
+				ctrl.control[actuator_controls_s::INDEX_THROTTLE]
+			);
 		}
 	}
 }

src/modules/commander/Commander.cpp

@@ -3821,6 +3821,8 @@ void Commander::battery_status_check()
 	// oldest timestamp.
 	hrt_abstime oldest_update = hrt_absolute_time();
 
+	_battery_current = 0.0f;
+
 	// Only iterate over connected batteries. We don't care if a disconnected battery is not regularly publishing.
 	for (size_t i = 0; i < num_connected_batteries; i++) {
 		if (batteries[i].warning > worst_warning) {
@@ -3831,9 +3833,8 @@ void Commander::battery_status_check()
 			oldest_update = batteries[i].timestamp;
 		}
 
-		if (batteries[i].system_source) {
-			_battery_current = batteries[i].current_filtered_a;
-		}
+		// Sum up current from all batteries.
+		_battery_current += batteries[i].current_filtered_a;
 	}
 
 	bool battery_warning_level_increased_while_armed = false;

src/modules/esc_battery/EscBattery.cpp

@@ -103,9 +103,7 @@ EscBattery::Run()
 		average_voltage_v /= esc_status.esc_count;
 
 		const bool connected = true;
-		const bool selected_source = true;
 		const int priority = 0;
-		const bool should_publish = true;
 
 		actuator_controls_s ctrl;
 		_actuator_ctrl_0_sub.copy(&ctrl);
@@ -115,10 +113,9 @@ EscBattery::Run()
 			average_voltage_v,
 			total_current_a,
 			connected,
-			selected_source,
+			battery_status_s::BATTERY_SOURCE_ESCS,
 			priority,
-			ctrl.control[actuator_controls_s::INDEX_THROTTLE],
-			should_publish);
+			ctrl.control[actuator_controls_s::INDEX_THROTTLE]);
 		_battery.publish();
 	}
 }

src/modules/simulator/simulator_mavlink.cpp

@@ -2,7 +2,7 @@
  *
  *   Copyright (c) 2015 Mark Charlebois. All rights reserved.
  *   Copyright (c) 2016 Anton Matosov. All rights reserved.
- *   Copyright (c) 2017-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -366,7 +366,8 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg)
 		vbatt *= _battery.cell_count();
 
 		const float throttle = 0.0f; // simulate no throttle compensation to make the estimate predictable
-		_battery.updateBatteryStatus(now_us, vbatt, ibatt, true, true, 0, throttle, true);
+		_battery.updateBatteryStatus(now_us, vbatt, ibatt, true, battery_status_s::BATTERY_SOURCE_POWER_MODULE,
+					     0, throttle);
 
 		_last_battery_timestamp = now_us;
 	}