mirror of https://github.com/ArduPilot/ardupilot
AP_BattMonitor: Change from division to multiplication
This commit is contained in:
parent
aabbda3978
commit
9d321c5c31
|
@ -75,7 +75,7 @@ void AP_BattMonitor_Backend::update_resistance_estimate()
|
|||
|
||||
// calculate time since last update
|
||||
uint32_t now = AP_HAL::millis();
|
||||
float loop_interval = (now - _resistance_timer_ms) / 1000.0f;
|
||||
float loop_interval = (now - _resistance_timer_ms) * 0.001f;
|
||||
_resistance_timer_ms = now;
|
||||
|
||||
// estimate short-term resistance
|
||||
|
|
|
@ -177,7 +177,7 @@ void AP_BattMonitor_Bebop::read(void)
|
|||
}
|
||||
|
||||
/* get battery voltage observed by cypress */
|
||||
vbat_raw = (float)data.batt_mv / 1000.0f;
|
||||
vbat_raw = (float)data.batt_mv * 0.001f;
|
||||
|
||||
/* do not compute battery status on ramping or braking transition */
|
||||
if (data.status == BEBOP_BLDC_STATUS_RAMPING ||
|
||||
|
@ -214,7 +214,7 @@ void AP_BattMonitor_Bebop::read(void)
|
|||
_state.voltage = vbat;
|
||||
_state.last_time_micros = tnow;
|
||||
_state.healthy = true;
|
||||
_state.consumed_mah = capacity - (remaining * capacity) / 100.0f;
|
||||
_state.consumed_mah = capacity - (remaining * capacity) * 0.01f;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -60,7 +60,7 @@ void AP_BattMonitor_SMBus_Generic::timer()
|
|||
|
||||
// read voltage (V)
|
||||
if (read_word(BATTMONITOR_SMBUS_VOLTAGE, data)) {
|
||||
_state.voltage = (float)data / 1000.0f;
|
||||
_state.voltage = (float)data * 0.001f;
|
||||
_state.last_time_micros = tnow;
|
||||
_state.healthy = true;
|
||||
}
|
||||
|
@ -113,7 +113,7 @@ void AP_BattMonitor_SMBus_Generic::timer()
|
|||
|
||||
// read current (A)
|
||||
if (read_word(BATTMONITOR_SMBUS_CURRENT, data)) {
|
||||
_state.current_amps = -(float)((int16_t)data) / 1000.0f;
|
||||
_state.current_amps = -(float)((int16_t)data) * 0.001f;
|
||||
_state.last_time_micros = tnow;
|
||||
}
|
||||
|
||||
|
|
|
@ -66,7 +66,7 @@ void AP_BattMonitor_SMBus_Solo::timer()
|
|||
|
||||
// read current
|
||||
if (read_block(BATTMONITOR_SMBUS_SOLO_CURRENT, buff, 4) == 4) {
|
||||
_state.current_amps = -(float)((int32_t)((uint32_t)buff[3]<<24 | (uint32_t)buff[2]<<16 | (uint32_t)buff[1]<<8 | (uint32_t)buff[0])) / 1000.0f;
|
||||
_state.current_amps = -(float)((int32_t)((uint32_t)buff[3]<<24 | (uint32_t)buff[2]<<16 | (uint32_t)buff[1]<<8 | (uint32_t)buff[0])) * 0.001f;
|
||||
_state.last_time_micros = tnow;
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue