GCS_MAVLink: report temperature information to the battery status report

This commit is contained in:
Michael du Breuil 2017-04-08 01:25:59 -07:00 committed by Francisco Ferreira
parent 9cb0d8f99b
commit 341ac701d1

View File

@ -240,11 +240,13 @@ void GCS_MAVLINK::send_battery_status(const AP_BattMonitor &battery, const uint8
static_assert(sizeof(AP_BattMonitor::cells) >= (sizeof(uint16_t) * MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN),
"Not enough battery cells for the MAVLink message");
float temp;
bool got_temperature = battery.get_temperature(temp, instance);
mavlink_msg_battery_status_send(chan,
instance, // id
MAV_BATTERY_FUNCTION_UNKNOWN, // function
MAV_BATTERY_TYPE_UNKNOWN, // type
INT16_MAX, // temperature. INT16_MAX if unknown
got_temperature ? ((int16_t) (temp * 100)) : INT16_MAX, // temperature. INT16_MAX if unknown
battery.get_cell_voltages(instance).cells, // cell voltages
battery.has_current(instance) ? battery.current_amps(instance) * 100 : -1, // current
battery.has_current(instance) ? battery.current_total_mah(instance) : -1, // total current