diff options
author | Andrew Tridgell <tridge@samba.org> | 2013-09-12 15:00:34 +1000 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-12 09:24:23 +0200 |
commit | f7c3ed3ed601c981615f791e68aff68d119b7c54 (patch) | |
tree | fdfd99a830b4fa4e00d39041e511f8ebd6e75176 /src | |
parent | 1f19a27e3cd5d0686dd65ecad6a171d025058b7c (diff) | |
download | px4-firmware-f7c3ed3ed601c981615f791e68aff68d119b7c54.tar.gz px4-firmware-f7c3ed3ed601c981615f791e68aff68d119b7c54.tar.bz2 px4-firmware-f7c3ed3ed601c981615f791e68aff68d119b7c54.zip |
px4io: split io_handle_battery() out from io_handle_status()
ready to add vservo and rssi
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 86 |
1 files changed, 52 insertions, 34 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 78d1d3e63..af20e61cb 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -409,9 +409,18 @@ private: */ int io_handle_alarms(uint16_t alarms); + /** + * Handle a battery update from IO. + * + * Publish IO battery information if necessary. + * + * @param vbatt vbattery register + * @param status ibatter register + */ + void io_handle_battery(uint16_t vbatt, uint16_t ibatt); + }; - namespace { @@ -1158,6 +1167,45 @@ PX4IO::io_handle_alarms(uint16_t alarms) return 0; } +void +PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) +{ + /* only publish if battery has a valid minimum voltage */ + if (vbatt <= 3300) { + return; + } + + battery_status_s battery_status; + battery_status.timestamp = hrt_absolute_time(); + + /* voltage is scaled to mV */ + battery_status.voltage_v = vbatt / 1000.0f; + + /* + ibatt contains the raw ADC count, as 12 bit ADC + value, with full range being 3.3v + */ + battery_status.current_a = ibatt * (3.3f/4096.0f) * _battery_amp_per_volt; + battery_status.current_a += _battery_amp_bias; + + /* + integrate battery over time to get total mAh used + */ + if (_battery_last_timestamp != 0) { + _battery_mamphour_total += battery_status.current_a * + (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600; + } + battery_status.discharged_mah = _battery_mamphour_total; + _battery_last_timestamp = battery_status.timestamp; + + /* lazily publish the battery voltage */ + if (_to_battery > 0) { + orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); + } else { + _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } +} + int PX4IO::io_get_status() { @@ -1171,40 +1219,10 @@ PX4IO::io_get_status() io_handle_status(regs[0]); io_handle_alarms(regs[1]); - - /* only publish if battery has a valid minimum voltage */ - if (regs[2] > 3300) { - battery_status_s battery_status; - - battery_status.timestamp = hrt_absolute_time(); - - /* voltage is scaled to mV */ - battery_status.voltage_v = regs[2] / 1000.0f; - /* - regs[3] contains the raw ADC count, as 12 bit ADC - value, with full range being 3.3v - */ - battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt; - battery_status.current_a += _battery_amp_bias; - - /* - integrate battery over time to get total mAh used - */ - if (_battery_last_timestamp != 0) { - _battery_mamphour_total += battery_status.current_a * - (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600; - } - battery_status.discharged_mah = _battery_mamphour_total; - _battery_last_timestamp = battery_status.timestamp; - - /* lazily publish the battery voltage */ - if (_to_battery > 0) { - orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); - } else { - _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); - } - } +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + io_handle_battery(regs[2], regs[3]); +#endif return ret; } |