@@ -73,19 +73,19 @@ class MavlinkStreamBatteryStatusV2 : public MavlinkStream
7373 && PX4_ISFINITE (battery_status.temperature )) ? static_cast <int16_t >(battery_status.temperature * 1e2f) : INT16_MAX;
7474 bat_msg.voltage = (battery_status.connected
7575 && PX4_ISFINITE (battery_status.voltage_v )
76- && battery_status.voltage_v != 0 . 0f ) ? battery_status.voltage_v : float (NAN);
76+ && ! math::isZero ( battery_status.voltage_v ) ) ? battery_status.voltage_v : float (NAN);
7777 bat_msg.current = (battery_status.connected
7878 && PX4_ISFINITE (battery_status.current_a )
79- && battery_status.current_a != -1 .0f ) ? battery_status.current_a : float (NAN);
79+ && fabsf ( battery_status.current_a - ( -1 .0f )) > FLT_EPSILON ) ? battery_status.current_a : float (NAN);
8080 bat_msg.capacity_consumed = (battery_status.connected
8181 && PX4_ISFINITE (battery_status.discharged_mah )
82- && battery_status.discharged_mah != -1.0 ) ? battery_status.discharged_mah * 1e-3f : float (NAN);
83- bat_msg.capacity_remaining = (battery_status.connected && PX4_ISFINITE (battery_status.remaining )
84- && battery_status.remaining != - 1 . 0f
85- && PX4_ISFINITE ( static_cast < float >( battery_status.capacity ) )) ?
86- battery_status.remaining * ( static_cast < float >( battery_status.capacity ) * 1e- 3f ) : float (NAN);
82+ && fabsf ( battery_status.discharged_mah - ( -1 .0f )) > FLT_EPSILON ) ? battery_status.discharged_mah * 1e-3f : float (NAN);
83+ bat_msg.capacity_remaining = (battery_status.connected && PX4_ISFINITE (battery_status.remaining_capacity_wh )
84+ && PX4_ISFINITE ( battery_status.nominal_voltage )
85+ && ! math::isZero ( battery_status.nominal_voltage )) ?
86+ battery_status.remaining_capacity_wh / battery_status.nominal_voltage : float (NAN);
8787 bat_msg.percent_remaining = (battery_status.connected && PX4_ISFINITE (battery_status.remaining )
88- && battery_status.remaining != -1 .0f ) ?
88+ && fabsf ( battery_status.remaining - ( -1 .0f )) > FLT_EPSILON ) ?
8989 static_cast <uint8_t >(roundf (battery_status.remaining * 1e2f)) : UINT8_MAX;
9090
9191 bat_msg.status_flags = get_status_flags (battery_status);
0 commit comments