Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_FrSkyTelem: Refactor battery current interface

Browse Source
This commit is contained in:
Michael du Breuil 2019-07-07 14:37:28 +00:00 committed by WickedShell
parent 58dbbd6d0e
commit ea82d6ea4c
1 changed files with 22 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -214,8 +214,14 @@ void AP_Frsky_Telem::send_SPort(void)
send_uint32(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage send_uint32(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage
break; break;
case 2: case 2:
send_uint32(DATA_ID_CURRENT, (uint16_t)roundf(_battery.current_amps() * 10.0f)); // send current consumption {
break; float current;
if (!_battery.current_amps(current)) {
current = 0;
}
send_uint32(DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption
break;
}
} }
if (_SPort.fas_call++ > 2) _SPort.fas_call = 0; if (_SPort.fas_call++ > 2) _SPort.fas_call = 0;
break; break;
@ -306,7 +312,11 @@ void AP_Frsky_Telem::send_D(void)
send_uint16(DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode send_uint16(DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode
send_uint16(DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining send_uint16(DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining
send_uint16(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage send_uint16(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage
send_uint16(DATA_ID_CURRENT, (uint16_t)roundf(_battery.current_amps() * 10.0f)); // send current consumption float current;
if (!_battery.current_amps(current)) {
current = 0;
}
send_uint16(DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption
calc_nav_alt(); calc_nav_alt();
send_uint16(DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send nav altitude integer part send_uint16(DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send nav altitude integer part
send_uint16(DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send nav altitude decimal part send_uint16(DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send nav altitude decimal part
@ -684,13 +694,20 @@ uint32_t AP_Frsky_Telem::calc_batt(uint8_t instance)
const AP_BattMonitor &_battery = AP::battery(); const AP_BattMonitor &_battery = AP::battery();
uint32_t batt; uint32_t batt;
float current, consumed_mah;
if (!_battery.current_amps(current, instance)) {
current = 0;
}
if (!_battery.consumed_mah(consumed_mah, instance)) {
consumed_mah = 0;
}
// battery voltage in decivolts, can have up to a 12S battery (4.25Vx12S = 51.0V) // battery voltage in decivolts, can have up to a 12S battery (4.25Vx12S = 51.0V)
batt = (((uint16_t)roundf(_battery.voltage(instance) * 10.0f)) & BATT_VOLTAGE_LIMIT); batt = (((uint16_t)roundf(_battery.voltage(instance) * 10.0f)) & BATT_VOLTAGE_LIMIT);
// battery current draw in deciamps // battery current draw in deciamps
batt |= prep_number(roundf(_battery.current_amps(instance) * 10.0f), 2, 1)<<BATT_CURRENT_OFFSET; batt |= prep_number(roundf(current * 10.0f), 2, 1)<<BATT_CURRENT_OFFSET;
// battery current drawn since power on in mAh (limit to 32767 (0x7FFF) since value is stored on 15 bits) // battery current drawn since power on in mAh (limit to 32767 (0x7FFF) since value is stored on 15 bits)
batt |= ((_battery.consumed_mah(instance) < BATT_TOTALMAH_LIMIT) ? ((uint16_t)roundf(_battery.consumed_mah(instance)) & BATT_TOTALMAH_LIMIT) : BATT_TOTALMAH_LIMIT)<<BATT_TOTALMAH_OFFSET; batt |= ((consumed_mah < BATT_TOTALMAH_LIMIT) ? ((uint16_t)roundf(consumed_mah) & BATT_TOTALMAH_LIMIT) : BATT_TOTALMAH_LIMIT)<<BATT_TOTALMAH_OFFSET;
return batt; return batt;
} }