|
|
|
@ -30,15 +30,16 @@ AP_BattMonitor_Backend::AP_BattMonitor_Backend(AP_BattMonitor &mon, AP_BattMonit
@@ -30,15 +30,16 @@ AP_BattMonitor_Backend::AP_BattMonitor_Backend(AP_BattMonitor &mon, AP_BattMonit
|
|
|
|
|
{ |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/// capacity_remaining_pct - returns the % battery capacity remaining (0 ~ 100)
|
|
|
|
|
uint8_t AP_BattMonitor_Backend::capacity_remaining_pct() const |
|
|
|
|
/// capacity_remaining_pct - returns true if the battery % is available and writes to the percentage argument
|
|
|
|
|
bool AP_BattMonitor_Backend::capacity_remaining_pct(uint8_t &percentage) const |
|
|
|
|
{ |
|
|
|
|
float mah_remaining = _params._pack_capacity - _state.consumed_mah; |
|
|
|
|
if ( _params._pack_capacity > 10 ) { // a very very small battery
|
|
|
|
|
return MIN(MAX((100 * (mah_remaining) / _params._pack_capacity), 0), UINT8_MAX); |
|
|
|
|
} else { |
|
|
|
|
return 0; |
|
|
|
|
// we consider anything under 10 mAh as being an invalid capacity and so will be our measurement of remaining capacity
|
|
|
|
|
if ( _params._pack_capacity <= 10) { |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
const float mah_remaining = _params._pack_capacity - _state.consumed_mah; |
|
|
|
|
percentage = constrain_float(100 * mah_remaining / _params._pack_capacity, 0, UINT8_MAX); |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// update battery resistance estimate
|
|
|
|
|