diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 898326287f..738732436a 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -7,7 +7,7 @@ float32 average_current_a # Battery current average in amperes, -1 if unknown float32 discharged_mah # Discharged amount in mAh, -1 if unknown float32 remaining # From 1 to 0, -1 if unknown float32 scale # Power scaling factor, >= 1, or -1 if unknown -float32 temperature # temperature of the battery +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 diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index e550c56c5b..7d9a2e506f 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -97,6 +97,8 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre battery_status->system_source = selected_source; battery_status->priority = priority; } + + battery_status->temperature = NAN; } void diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 64555ec531..de24336515 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -704,7 +705,10 @@ protected: bat_msg.energy_consumed = -1; bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1; bat_msg.battery_remaining = (battery_status.connected) ? ceilf(battery_status.remaining * 100.0f) : -1; - bat_msg.temperature = (battery_status.connected) ? (int16_t)battery_status.temperature : INT16_MAX; + + + bool temperature_valid = battery_status.connected && PX4_ISFINITE(battery_status.temperature); + bat_msg.temperature = temperature_valid ? (int16_t)(battery_status.temperature * 100.0f) : INT16_MAX; //bat_msg.average_current_battery = (battery_status.connected) ? battery_status.average_current_a * 100.0f : -1; //bat_msg.serial_number = (battery_status.connected) ? battery_status.serial_number : 0; //bat_msg.capacity = (battery_status.connected) ? battery_status.capacity : 0;