From d7e04a361909fb65839fb8f2ab515ea17e5f6d77 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 2 May 2013 21:28:55 +1000 Subject: px4io: fixed voltage/current output and add discharged_mah calculation this integrates the current over time to calculate discharged_mah, and allows the scaling of the current and the bias to be set with the px4io command --- src/drivers/px4io/px4io.cpp | 70 +++++++++++++++++++++++++++++++++++++-------- 1 file changed, 58 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d8b5c51c4..b4703839b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -108,6 +108,14 @@ public: */ int set_update_rate(int rate); + /** + * Set the battery current scaling and bias + * + * @param amp_per_volt + * @param amp_bias + */ + void set_battery_current_scaling(float amp_per_volt, float amp_bias); + /** * Print the current status of IO */ @@ -151,6 +159,10 @@ private: bool _primary_pwm_device; ///< true if we are the default PWM output + float _battery_amp_per_volt; + float _battery_amp_bias; + float _battery_mamphour_total; + uint64_t _battery_last_timestamp; /** * Trampoline to the worker task @@ -314,6 +326,10 @@ PX4IO::PX4IO() : _to_actuators_effective(0), _to_outputs(0), _to_battery(0), + _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor + _battery_amp_bias(0), + _battery_mamphour_total(0), + _battery_last_timestamp(0), _primary_pwm_device(false) { /* we need this potentially before it could be set in task_main */ @@ -884,11 +900,22 @@ PX4IO::io_get_status() /* voltage is scaled to mV */ battery_status.voltage_v = regs[2] / 1000.0f; - /* current scaling should be to cA in order to avoid limiting at 65A */ - battery_status.current_a = regs[3] / 100.f; + /* + 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; - /* this requires integration over time - not currently implemented */ - battery_status.discharged_mah = -1.0f; + /* + 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) { @@ -1245,9 +1272,14 @@ PX4IO::print_status() ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : "")); - printf("vbatt %u ibatt %u\n", - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT)); + printf("vbatt %u ibatt %u vbatt scale %u\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); + printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n", + _battery_amp_per_volt, + _battery_amp_bias, + _battery_mamphour_total); printf("actuators"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); @@ -1288,10 +1320,6 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); - printf("vbatt scale %u ibatt scale %u ibatt bias %u\n", - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS)); printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); printf("controls"); for (unsigned i = 0; i < _max_controls; i++) @@ -1521,6 +1549,12 @@ PX4IO::set_update_rate(int rate) return 0; } +void +PX4IO::set_battery_current_scaling(float amp_per_volt, float amp_bias) +{ + _battery_amp_per_volt = amp_per_volt; + _battery_amp_bias = amp_bias; +} extern "C" __EXPORT int px4io_main(int argc, char *argv[]); @@ -1658,6 +1692,18 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "current")) { + if (g_dev != nullptr) { + if ((argc > 3)) { + g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3])); + } else { + errx(1, "missing argument (apm_per_volt, amp_offset)"); + return 1; + } + } + exit(0); + } + if (!strcmp(argv[1], "recovery")) { if (g_dev != nullptr) { @@ -1785,5 +1831,5 @@ px4io_main(int argc, char *argv[]) monitor(); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current' or 'update'"); } -- cgit v1.2.3