commander / mavlink: Add battery status to output BATTERY_STATUS MAVLink message

msg/vehicle_status.msg

@@ -147,6 +147,8 @@ float32 load					# processor load from 0 to 1
 float32 battery_voltage
 float32 battery_current
 float32 battery_remaining
+float32 battery_discharged_mah
+uint32 battery_cell_count
 
 uint8 battery_warning    # current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum
 uint16 drop_rate_comm

src/modules/commander/commander.cpp

@@ -1502,7 +1502,9 @@ int commander_thread_main(int argc, char *argv[])
 			if (hrt_absolute_time() > commander_boot_timestamp + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
 				status.battery_current = battery.current_a;
+				status.battery_discharged_mah = battery.discharged_mah;
 				status.condition_battery_voltage_valid = true;
+				status.battery_cell_count = battery_get_n_cells();
 
 				/* get throttle (if armed), as we only care about energy negative throttle also counts */
 				float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;

src/modules/commander/commander_helper.cpp

@@ -330,6 +330,10 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
 	ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
 }
 
+unsigned battery_get_n_cells() {
+	return bat_n_cells;
+}
+
 float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized)
 {
 	float ret = 0;

src/modules/commander/commander_helper.h

@@ -89,4 +89,6 @@ int battery_init();
  */
 float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized);
 
+unsigned battery_get_n_cells();
+
 #endif /* COMMANDER_HELPER_H_ */

src/modules/mavlink/mavlink_messages.cpp

@@ -543,6 +543,27 @@ protected:
 							status.battery_remaining * 100.0f : -1;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg);
+
+			/* battery status message with higher resolution */
+			mavlink_battery_status_t bat_msg;
+			bat_msg.id = 0;
+			bat_msg.battery_function = MAV_BATTERY_FUNCTION_ALL;
+			bat_msg.type = MAV_BATTERY_TYPE_LIPO;
+			bat_msg.temperature = INT16_MAX;
+			for (unsigned i = 0; i < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); i++) {
+				if (i < status.battery_cell_count) {
+					bat_msg.voltages[i] = (status.battery_voltage / status.battery_cell_count) * 1000.0f;
+				} else {
+					bat_msg.voltages[i] = 0;
+				}
+			}
+			bat_msg.current_battery = status.battery_current * 100.0f;
+			bat_msg.current_consumed = status.battery_discharged_mah;
+			bat_msg.energy_consumed = -1.0f;
+			bat_msg.battery_remaining = (status.battery_voltage > 0) ?
+							status.battery_remaining * 100.0f : -1;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_BATTERY_STATUS, &bat_msg);
 		}
 	}
 };