mirror of https://github.com/ArduPilot/ardupilot
AP_BattMonitor: zero consumption total before recalculating
add reset function to ESC backend.
This commit is contained in:
parent
534b807e22
commit
eea2151774
|
@ -35,6 +35,7 @@ void AP_BattMonitor_ESC::read(void)
|
|||
float current_sum = 0;
|
||||
float temperature_sum = 0;
|
||||
uint32_t highest_ms = 0;
|
||||
_state.consumed_mah = delta_mah;
|
||||
|
||||
for (uint8_t i=0; i<ESC_TELEM_MAX_ESCS; i++) {
|
||||
int16_t temperature_cdeg;
|
||||
|
@ -92,4 +93,17 @@ void AP_BattMonitor_ESC::read(void)
|
|||
}
|
||||
}
|
||||
|
||||
bool AP_BattMonitor_ESC::reset_remaining(float percentage)
|
||||
{
|
||||
delta_mah = 0.0f;
|
||||
read();
|
||||
const float current_mah = _state.consumed_mah;
|
||||
if (AP_BattMonitor_Backend::reset_remaining(percentage)) {
|
||||
delta_mah = _state.consumed_mah - current_mah;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
#endif // HAL_WITH_ESC_TELEM
|
||||
|
|
|
@ -43,9 +43,13 @@ public:
|
|||
// ESC_Telem provides temperature info
|
||||
bool has_temperature() const override { return have_temperature; };
|
||||
|
||||
// reset remaining percentage to given value
|
||||
virtual bool reset_remaining(float percentage) override;
|
||||
|
||||
private:
|
||||
bool have_current;
|
||||
bool have_temperature;
|
||||
float delta_mah;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
Loading…
Reference in New Issue