Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Frsky_Telem: add second battery info

This commit is contained in:
Florent Martel 2017-10-10 08:42:47 -05:00 committed by Tom Pittenger
parent 5d9299d72f
commit fee79afb8e
2 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -123,10 +123,17 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
return;
}
if ((now - _passthrough.batt_timer) >= 1000) {
send_uint32(DIY_FIRST_ID+3, calc_batt());
send_uint32(DIY_FIRST_ID+3, calc_batt(0));
_passthrough.batt_timer = AP_HAL::millis();
return;
}
if (_battery.num_instances() > 1) {
if ((now - _passthrough.batt_timer2) >= 1000) {
send_uint32(DIY_FIRST_ID+8, calc_batt(1));
_passthrough.batt_timer2 = AP_HAL::millis();
return;
}
}
if ((now - _passthrough.gps_status_timer) >= 1000) {
send_uint32(DIY_FIRST_ID+2, calc_gps_status());
_passthrough.gps_status_timer = AP_HAL::millis();
@ -652,16 +659,16 @@ uint32_t AP_Frsky_Telem::calc_gps_status(void)
* prepare battery data
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/
uint32_t AP_Frsky_Telem::calc_batt(void)
uint32_t AP_Frsky_Telem::calc_batt(uint8_t instance)
{
uint32_t batt;
// battery voltage in decivolts, can have up to a 12S battery (4.25Vx12S = 51.0V)
batt = (((uint16_t)roundf(_battery.voltage() * 10.0f)) & BATT_VOLTAGE_LIMIT);
batt = (((uint16_t)roundf(_battery.voltage(instance) * 10.0f)) & BATT_VOLTAGE_LIMIT);
// battery current draw in deciamps
batt |= prep_number(roundf(_battery.current_amps() * 10.0f), 2, 1)<<BATT_CURRENT_OFFSET;
batt |= prep_number(roundf(_battery.current_amps(instance) * 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)
batt |= ((_battery.current_total_mah() < BATT_TOTALMAH_LIMIT) ? ((uint16_t)roundf(_battery.current_total_mah()) & BATT_TOTALMAH_LIMIT) : BATT_TOTALMAH_LIMIT)<<BATT_TOTALMAH_OFFSET;
batt |= ((_battery.current_total_mah(instance) < BATT_TOTALMAH_LIMIT) ? ((uint16_t)roundf(_battery.current_total_mah(instance)) & BATT_TOTALMAH_LIMIT) : BATT_TOTALMAH_LIMIT)<<BATT_TOTALMAH_OFFSET;
return batt;
}

3
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
View File

@ -198,6 +198,7 @@ private:
uint32_t params_timer;
uint32_t ap_status_timer;
uint32_t batt_timer;
uint32_t batt_timer2;
uint32_t gps_status_timer;
uint32_t home_timer;
uint32_t velandyaw_timer;
@ -249,7 +250,7 @@ private:
uint32_t calc_param(void);
uint32_t calc_gps_latlng(bool *send_latitude);
uint32_t calc_gps_status(void);
uint32_t calc_batt(void);
uint32_t calc_batt(uint8_t instance);
uint32_t calc_ap_status(void);
uint32_t calc_home(void);
uint32_t calc_velandyaw(void);