diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index 58f0ece0af..daddf1f70b 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp @@ -288,7 +288,7 @@ Syslink::task_main() _memory = new SyslinkMemory(this); _memory->init(); - _battery.reset(&_battery_status); + _battery.reset(); // int ret; @@ -411,7 +411,7 @@ Syslink::handle_message(syslink_message_t *msg) memcpy(&vbat, &msg->data[1], sizeof(float)); //memcpy(&iset, &msg->data[5], sizeof(float)); - _battery.updateBatteryStatus(t, vbat, -1, true, true, 0, 0, false, &_battery_status); + _battery.updateBatteryStatus(vbat, -1, t, true, 0, 0, false); // Update battery charge state @@ -427,9 +427,6 @@ Syslink::handle_message(syslink_message_t *msg) _bstate = BAT_DISCHARGING; } - // announce the battery status if needed, just publish else - _battery_pub.publish(_battery_status); - } else if (msg->type == SYSLINK_RADIO_RSSI) { uint8_t rssi = msg->data[0]; // Between 40 and 100 meaning -40 dBm to -100 dBm _rssi = 140 - rssi * 100 / (100 - 40); diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.h b/boards/bitcraze/crazyflie/syslink/syslink_main.h index 79bdbce73f..d8495f1750 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.h +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.h @@ -135,12 +135,9 @@ private: hrt_abstime _params_update[3]; // Time at which the parameters were updated hrt_abstime _params_ack[3]; // Time at which the parameters were acknowledged by the nrf module - uORB::PublicationMulti _battery_pub{ORB_ID(battery_status)}; uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; - struct battery_status_s _battery_status; - - Battery _battery; + Battery1 _battery; int32_t _rssi; battery_state _bstate; diff --git a/src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp index 7feaab4aaa..0d54d2e38d 100644 --- a/src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp +++ b/src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp @@ -101,7 +101,7 @@ private: orb_advert_t _battery_topic; orb_advert_t _esc_topic; - Battery _battery; + Battery1 _battery; bool _armed; float _last_throttle; @@ -201,14 +201,8 @@ int DfBebopBusWrapper::set_esc_speeds(const float speed_scaled[4]) int DfBebopBusWrapper::_publish(struct bebop_state_data &data) { - battery_status_s battery_report; const hrt_abstime timestamp = hrt_absolute_time(); - // TODO Check if this is the right way for the Bebop - // We don't have current measurements - _battery.updateBatteryStatus(timestamp, data.battery_voltage_v, 0.0, true, true, 0, _last_throttle, _armed, - &battery_report); - esc_status_s esc_status = {}; uint16_t esc_speed_setpoint_rpm[4] = {}; @@ -224,13 +218,9 @@ int DfBebopBusWrapper::_publish(struct bebop_state_data &data) // TODO: when is this ever blocked? if (!(m_pub_blocked)) { - if (_battery_topic == nullptr) { - _battery_topic = orb_advertise_multi(ORB_ID(battery_status), &battery_report, - &_battery_orb_class_instance, ORB_PRIO_LOW); - - } else { - orb_publish(ORB_ID(battery_status), _battery_topic, &battery_report); - } + // TODO Check if this is the right way for the Bebop + // We don't have current measurements + _battery.updateBatteryStatus(data.battery_voltage_v, 0.0, timestamp, true, 0, _last_throttle, _armed); if (_esc_topic == nullptr) { _esc_topic = orb_advertise(ORB_ID(esc_status), &esc_status); diff --git a/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp index 9f70083adf..3428f6112c 100644 --- a/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp +++ b/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp @@ -48,7 +48,6 @@ #include #include -#include #include #include @@ -70,9 +69,7 @@ public: private: int _publish(const struct ltc2946_sensor_data &data); - orb_advert_t _battery_pub{nullptr}; - battery_status_s _battery_status{}; - Battery _battery{}; + Battery1 _battery{}; int _actuator_ctrl_0_sub{-1}; int _vcontrol_mode_sub{-1}; @@ -82,7 +79,7 @@ private: DfLtc2946Wrapper::DfLtc2946Wrapper() : LTC2946(LTC2946_DEVICE_PATH) { - _battery.reset(&_battery_status); + _battery.reset(); // subscriptions _actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0)); @@ -134,21 +131,16 @@ int DfLtc2946Wrapper::stop() int DfLtc2946Wrapper::_publish(const struct ltc2946_sensor_data &data) { hrt_abstime t = hrt_absolute_time(); - bool connected = data.battery_voltage_V > BOARD_ADC_OPEN_CIRCUIT_V; actuator_controls_s ctrl; orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl); vehicle_control_mode_s vcontrol_mode; orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); - _battery.updateBatteryStatus(t, data.battery_voltage_V, data.battery_current_A, - connected, true, 1, + _battery.updateBatteryStatus(data.battery_voltage_V, data.battery_current_A, t, + true, 1, ctrl.control[actuator_controls_s::INDEX_THROTTLE], - vcontrol_mode.flag_armed, &_battery_status); - - int instance; - orb_publish_auto(ORB_ID(battery_status), &_battery_pub, &_battery_status, &instance, ORB_PRIO_DEFAULT); - + vcontrol_mode.flag_armed); return 0; } diff --git a/src/lib/battery/CMakeLists.txt b/src/lib/battery/CMakeLists.txt index aaca5a3547..de7ac02ab2 100644 --- a/src/lib/battery/CMakeLists.txt +++ b/src/lib/battery/CMakeLists.txt @@ -31,4 +31,4 @@ # ############################################################################ -px4_add_library(battery battery.cpp) +px4_add_library(battery battery_base.cpp) diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 4fbe58ee20..bda9f09d88 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016, 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,69 +31,52 @@ * ****************************************************************************/ -/** - * @file battery.h - * - * Library calls for battery functionality. - * - * @author Julian Oes - */ - #pragma once -#include -#include -#include +/** + * @file battery.h + * Implementations of BatteryBase + * + * The multiple batteries all share the same logic for calibration. The only difference is which parameters are used + * (Battery 1 uses `BAT_*`, while Battery 2 uses `BAT2_*`). To avoid code duplication, inheritance is being used. + * The problem is that the `ModuleParams` class depends on a macro which defines member variables. You can't override + * member variables in C++, so we have to declare virtual getter functions in BatteryBase, and implement them here. + * + * The alternative would be to avoid ModuleParams entirely, and build parameter names dynamically, like so: + * ``` + * char param_name[17]; //16 max length of parameter name, + null terminator + * int battery_index = 1; // Or 2 or 3 or whatever + * snprintf(param_name, 17, "BAT%d_N_CELLS", battery_index); + * // A real implementation would have to handle the case where battery_index == 1 and there is no number in the param name. + * param_find(param_name); // etc + * ``` + * + * This was decided against because the newer ModuleParams API provides more type safety and avoids code duplication. + * + * To add a new battery, just create a new implementation of BatteryBase and implement all of the _get_* methods, + * then add all of the new parameters necessary for calibration. + */ +#include "battery_base.h" -class Battery : public ModuleParams +class Battery1 : public BatteryBase { public: - Battery(); + Battery1() : BatteryBase() + { + // Can't do this in the constructor because virtual functions + if (_get_adc_channel() >= 0) { + vChannel = _get_adc_channel(); - /** - * Reset all battery stats and report invalid/nothing. - */ - void reset(battery_status_s *battery_status); + } else { + vChannel = DEFAULT_V_CHANNEL[0]; + } - /** - * Get the battery cell count - */ - int cell_count() { return _param_bat_n_cells.get(); } - - /** - * Get the empty voltage per cell - */ - float empty_cell_voltage() { return _param_bat_v_empty.get(); } - - /** - * Get the full voltage per cell - */ - float full_cell_voltage() { return _param_bat_v_charged.get(); } - - /** - * Update current battery status message. - * - * @param voltage_v: current voltage in V - * @param current_a: current current in A - * @param connected: Battery is connected - * @param selected_source: This battery is on the brick that the selected source for selected_source - * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417 - * @param throttle_normalized: throttle from 0 to 1 - */ - void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, - bool connected, bool selected_source, int priority, - float throttle_normalized, - bool armed, battery_status_s *status); + // TODO: Add parameter, like with V + iChannel = DEFAULT_I_CHANNEL[0]; + } private: - void filterVoltage(float voltage_v); - void filterThrottle(float throttle); - void filterCurrent(float current_a); - void sumDischarged(hrt_abstime timestamp, float current_a); - void estimateRemaining(float voltage_v, float current_a, float throttle, bool armed); - void determineWarning(bool connected); - void computeScale(); DEFINE_PARAMETERS( (ParamFloat) _param_bat_v_empty, @@ -102,20 +85,94 @@ private: (ParamFloat) _param_bat_capacity, (ParamFloat) _param_bat_v_load_drop, (ParamFloat) _param_bat_r_internal, + (ParamFloat) _param_v_div, + (ParamFloat) _param_a_per_v, + (ParamInt) _param_adc_channel, + (ParamFloat) _param_bat_low_thr, (ParamFloat) _param_bat_crit_thr, - (ParamFloat) _param_bat_emergen_thr + (ParamFloat) _param_bat_emergen_thr, + (ParamFloat) _param_cnt_v_volt, + (ParamFloat) _param_cnt_v_curr, + (ParamFloat) _param_v_offs_cur, + (ParamInt) _param_source ) - bool _battery_initialized = false; - float _voltage_filtered_v = -1.f; - float _throttle_filtered = -1.f; - float _current_filtered_a = -1.f; - float _discharged_mah = 0.f; - float _discharged_mah_loop = 0.f; - float _remaining_voltage = -1.f; ///< normalized battery charge level remaining based on voltage - float _remaining = -1.f; ///< normalized battery charge level, selected based on config param - float _scale = 1.f; - uint8_t _warning; - hrt_abstime _last_timestamp; + float _get_bat_v_empty() override {return _param_bat_v_empty.get(); } + float _get_bat_v_charged() override {return _param_bat_v_charged.get(); } + int _get_bat_n_cells() override {return _param_bat_n_cells.get(); } + float _get_bat_capacity() override {return _param_bat_capacity.get(); } + float _get_bat_v_load_drop() override {return _param_bat_v_load_drop.get(); } + float _get_bat_r_internal() override {return _param_bat_r_internal.get(); } + float _get_bat_low_thr() override {return _param_bat_low_thr.get(); } + float _get_bat_crit_thr() override {return _param_bat_crit_thr.get(); } + float _get_bat_emergen_thr() override {return _param_bat_emergen_thr.get(); } + float _get_cnt_v_volt_raw() override {return _param_cnt_v_volt.get(); } + float _get_cnt_v_curr_raw() override {return _param_cnt_v_curr.get(); } + float _get_v_offs_cur() override {return _param_v_offs_cur.get(); } + float _get_v_div_raw() override {return _param_v_div.get(); } + float _get_a_per_v_raw() override {return _param_a_per_v.get(); } + int _get_source() override {return _param_source.get(); } + int _get_adc_channel() override {return _param_adc_channel.get(); } + + int _get_brick_index() override {return 0; } +}; + +class Battery2 : public BatteryBase +{ +public: + Battery2() : BatteryBase() + { + // Can't do this in the constructor because virtual functions + if (_get_adc_channel() >= 0) { + vChannel = _get_adc_channel(); + + } else { + vChannel = DEFAULT_V_CHANNEL[1]; + } + + // TODO: Add parameter, like with V + iChannel = DEFAULT_I_CHANNEL[1]; + } + +private: + + DEFINE_PARAMETERS( + (ParamFloat) _param_bat_v_empty, + (ParamFloat) _param_bat_v_charged, + (ParamInt) _param_bat_n_cells, + (ParamFloat) _param_bat_capacity, + (ParamFloat) _param_bat_v_load_drop, + (ParamFloat) _param_bat_r_internal, + (ParamFloat) _param_v_div, + (ParamFloat) _param_a_per_v, + (ParamInt) _param_adc_channel, + + (ParamFloat) _param_bat_low_thr, + (ParamFloat) _param_bat_crit_thr, + (ParamFloat) _param_bat_emergen_thr, + (ParamFloat) _param_cnt_v_volt, + (ParamFloat) _param_cnt_v_curr, + (ParamFloat) _param_v_offs_cur, + (ParamInt) _param_source + ) + + float _get_bat_v_empty() override {return _param_bat_v_empty.get(); } + float _get_bat_v_charged() override {return _param_bat_v_charged.get(); } + int _get_bat_n_cells() override {return _param_bat_n_cells.get(); } + float _get_bat_capacity() override {return _param_bat_capacity.get(); } + float _get_bat_v_load_drop() override {return _param_bat_v_load_drop.get(); } + float _get_bat_r_internal() override {return _param_bat_r_internal.get(); } + float _get_bat_low_thr() override {return _param_bat_low_thr.get(); } + float _get_bat_crit_thr() override {return _param_bat_crit_thr.get(); } + float _get_bat_emergen_thr() override {return _param_bat_emergen_thr.get(); } + float _get_cnt_v_volt_raw() override {return _param_cnt_v_volt.get(); } + float _get_cnt_v_curr_raw() override {return _param_cnt_v_curr.get(); } + float _get_v_offs_cur() override {return _param_v_offs_cur.get(); } + float _get_v_div_raw() override {return _param_v_div.get(); } + float _get_a_per_v_raw() override {return _param_a_per_v.get(); } + int _get_source() override {return _param_source.get(); } + int _get_adc_channel() override {return _param_adc_channel.get(); } + + int _get_brick_index() override {return 1; } }; diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery_base.cpp similarity index 58% rename from src/lib/battery/battery.cpp rename to src/lib/battery/battery_base.cpp index 1cbf1709b2..02ea25396e 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery_base.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,19 +32,20 @@ ****************************************************************************/ /** - * @file battery.cpp + * @file battery_base.cpp * * Library calls for battery functionality. * * @author Julian Oes + * @author Timothy Scott */ -#include "battery.h" +#include "battery_base.h" #include #include #include -Battery::Battery() : +BatteryBase::BatteryBase() : ModuleParams(nullptr), _warning(battery_status_s::BATTERY_WARNING_NONE), _last_timestamp(0) @@ -52,26 +53,44 @@ Battery::Battery() : } void -Battery::reset(battery_status_s *battery_status) +BatteryBase::reset() { - memset(battery_status, 0, sizeof(*battery_status)); - battery_status->current_a = -1.f; - battery_status->remaining = 1.f; - battery_status->scale = 1.f; - battery_status->cell_count = _param_bat_n_cells.get(); + memset(&_battery_status, 0, sizeof(_battery_status)); + _battery_status.current_a = -1.f; + _battery_status.remaining = 1.f; + _battery_status.scale = 1.f; + _battery_status.cell_count = _get_bat_n_cells(); // TODO: check if it is sane to reset warning to NONE - battery_status->warning = battery_status_s::BATTERY_WARNING_NONE; - battery_status->connected = false; + _battery_status.warning = battery_status_s::BATTERY_WARNING_NONE; + _battery_status.connected = false; } void -Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, - bool connected, bool selected_source, int priority, - float throttle_normalized, - bool armed, battery_status_s *battery_status) +BatteryBase::updateBatteryStatusRawADC(int32_t voltage_raw, int32_t current_raw, hrt_abstime timestamp, + bool selected_source, int priority, + float throttle_normalized, + bool armed) { - reset(battery_status); - battery_status->timestamp = timestamp; + + float voltage_v = (voltage_raw * _get_cnt_v_volt()) * _get_v_div(); + float current_a = ((current_raw * _get_cnt_v_curr()) - _get_v_offs_cur()) * _get_a_per_v(); + + updateBatteryStatus(voltage_v, current_a, timestamp, selected_source, priority, throttle_normalized, armed); +} + +void +BatteryBase::updateBatteryStatus(float voltage_v, float current_a, hrt_abstime timestamp, + bool selected_source, int priority, + float throttle_normalized, + bool armed) +{ + updateParams(); + + bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V && + (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid()); + + reset(); + _battery_status.timestamp = timestamp; filterVoltage(voltage_v); filterThrottle(throttle_normalized); filterCurrent(current_a); @@ -85,24 +104,30 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre if (_voltage_filtered_v > 2.1f) { _battery_initialized = true; - battery_status->voltage_v = voltage_v; - battery_status->voltage_filtered_v = _voltage_filtered_v; - battery_status->scale = _scale; - battery_status->current_a = current_a; - battery_status->current_filtered_a = _current_filtered_a; - battery_status->discharged_mah = _discharged_mah; - battery_status->warning = _warning; - battery_status->remaining = _remaining; - battery_status->connected = connected; - battery_status->system_source = selected_source; - battery_status->priority = priority; + _battery_status.voltage_v = voltage_v; + _battery_status.voltage_filtered_v = _voltage_filtered_v; + _battery_status.scale = _scale; + _battery_status.current_a = current_a; + _battery_status.current_filtered_a = _current_filtered_a; + _battery_status.discharged_mah = _discharged_mah; + _battery_status.warning = _warning; + _battery_status.remaining = _remaining; + _battery_status.connected = connected; + _battery_status.system_source = selected_source; + _battery_status.priority = priority; + } + + _battery_status.timestamp = timestamp; + + if (_get_source() == 0) { + orb_publish_auto(ORB_ID(battery_status), &_orbAdvert, &_battery_status, &_orbInstance, ORB_PRIO_DEFAULT); } battery_status->temperature = NAN; } void -Battery::filterVoltage(float voltage_v) +BatteryBase::filterVoltage(float voltage_v) { if (!_battery_initialized) { _voltage_filtered_v = voltage_v; @@ -117,7 +142,7 @@ Battery::filterVoltage(float voltage_v) } void -Battery::filterCurrent(float current_a) +BatteryBase::filterCurrent(float current_a) { if (!_battery_initialized) { _current_filtered_a = current_a; @@ -131,7 +156,7 @@ Battery::filterCurrent(float current_a) } } -void Battery::filterThrottle(float throttle) +void BatteryBase::filterThrottle(float throttle) { if (!_battery_initialized) { _throttle_filtered = throttle; @@ -145,7 +170,7 @@ void Battery::filterThrottle(float throttle) } void -Battery::sumDischarged(hrt_abstime timestamp, float current_a) +BatteryBase::sumDischarged(hrt_abstime timestamp, float current_a) { // Not a valid measurement if (current_a < 0.f) { @@ -167,24 +192,24 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a) } void -Battery::estimateRemaining(float voltage_v, float current_a, float throttle, bool armed) +BatteryBase::estimateRemaining(float voltage_v, float current_a, float throttle, bool armed) { // remaining battery capacity based on voltage - float cell_voltage = voltage_v / _param_bat_n_cells.get(); + float cell_voltage = voltage_v / _get_bat_n_cells(); // correct battery voltage locally for load drop to avoid estimation fluctuations - if (_param_bat_r_internal.get() >= 0.f) { - cell_voltage += _param_bat_r_internal.get() * current_a; + if (_get_bat_r_internal() >= 0.f) { + cell_voltage += _get_bat_r_internal() * current_a; } else { // assume linear relation between throttle and voltage drop - cell_voltage += throttle * _param_bat_v_load_drop.get(); + cell_voltage += throttle * _get_bat_v_load_drop(); } - _remaining_voltage = math::gradual(cell_voltage, _param_bat_v_empty.get(), _param_bat_v_charged.get(), 0.f, 1.f); + _remaining_voltage = math::gradual(cell_voltage, _get_bat_v_empty(), _get_bat_v_charged(), 0.f, 1.f); // choose which quantity we're using for final reporting - if (_param_bat_capacity.get() > 0.f) { + if (_get_bat_capacity() > 0.f) { // if battery capacity is known, fuse voltage measurement with used capacity if (!_battery_initialized) { // initialization of the estimation state @@ -195,7 +220,7 @@ Battery::estimateRemaining(float voltage_v, float current_a, float throttle, boo const float weight_v = 3e-4f * (1 - _remaining_voltage); _remaining = (1 - weight_v) * _remaining + weight_v * _remaining_voltage; // directly apply current capacity slope calculated using current - _remaining -= _discharged_mah_loop / _param_bat_capacity.get(); + _remaining -= _discharged_mah_loop / _get_bat_capacity(); _remaining = math::max(_remaining, 0.f); } @@ -206,17 +231,17 @@ Battery::estimateRemaining(float voltage_v, float current_a, float throttle, boo } void -Battery::determineWarning(bool connected) +BatteryBase::determineWarning(bool connected) { if (connected) { // propagate warning state only if the state is higher, otherwise remain in current warning state - if (_remaining < _param_bat_emergen_thr.get()) { + if (_remaining < _get_bat_emergen_thr()) { _warning = battery_status_s::BATTERY_WARNING_EMERGENCY; - } else if (_remaining < _param_bat_crit_thr.get()) { + } else if (_remaining < _get_bat_crit_thr()) { _warning = battery_status_s::BATTERY_WARNING_CRITICAL; - } else if (_remaining < _param_bat_low_thr.get()) { + } else if (_remaining < _get_bat_low_thr()) { _warning = battery_status_s::BATTERY_WARNING_LOW; } else { @@ -226,14 +251,14 @@ Battery::determineWarning(bool connected) } void -Battery::computeScale() +BatteryBase::computeScale() { - const float voltage_range = (_param_bat_v_charged.get() - _param_bat_v_empty.get()); + const float voltage_range = (_get_bat_v_charged() - _get_bat_v_empty()); // reusing capacity calculation to get single cell voltage before drop - const float bat_v = _param_bat_v_empty.get() + (voltage_range * _remaining_voltage); + const float bat_v = _get_bat_v_empty() + (voltage_range * _remaining_voltage); - _scale = _param_bat_v_charged.get() / bat_v; + _scale = _get_bat_v_charged() / bat_v; if (_scale > 1.3f) { // Allow at most 30% compensation _scale = 1.3f; @@ -242,3 +267,55 @@ Battery::computeScale() _scale = 1.f; } } + +float +BatteryBase::_get_cnt_v_volt() +{ + float val = _get_cnt_v_volt_raw(); + + if (val < 0.0f) { + return 3.3f / 4096.0f; + + } else { + return val; + } +} + +float +BatteryBase::_get_cnt_v_curr() +{ + float val = _get_cnt_v_curr_raw(); + + if (val < 0.0f) { + return 3.3f / 4096.0f; + + } else { + return val; + } +} + +float +BatteryBase::_get_v_div() +{ + float val = _get_v_div_raw(); + + if (val <= 0.0f) { + return BOARD_BATTERY1_V_DIV; + + } else { + return val; + } +} + +float +BatteryBase::_get_a_per_v() +{ + float val = _get_a_per_v_raw(); + + if (val <= 0.0f) { + return BOARD_BATTERY1_A_PER_V; + + } else { + return val; + } +} diff --git a/src/lib/battery/battery_base.h b/src/lib/battery/battery_base.h new file mode 100644 index 0000000000..1eb2bc9e4f --- /dev/null +++ b/src/lib/battery/battery_base.h @@ -0,0 +1,199 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file battery_base.h + * + * Library calls for battery functionality. + * + * @author Julian Oes + * @author Timothy Scott + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +/** + * BatteryBase is a base class for any type of battery. + * + * See battery.h for example implementation, and for explanation of why this is designed like it is. + */ +class BatteryBase : ModuleParams +{ +public: + BatteryBase(); + + /** + * Reset all battery stats and report invalid/nothing. + */ + void reset(); + + /** + * Get the battery cell count + */ + int cell_count() { return _get_bat_n_cells(); } + + /** + * Get the empty voltage per cell + */ + float empty_cell_voltage() { return _get_bat_v_empty(); } + + /** + * Get the full voltage per cell + */ + float full_cell_voltage() { return _get_bat_v_charged(); } + + /** + * Update current battery status message. + * + * @param voltage_raw Battery voltage read from ADC, in raw ADC counts + * @param current_raw Voltage of current sense resistor, in raw ADC counts + * @param timestamp Time at which the ADC was read (use hrt_absolute_time()) + * @param selected_source This battery is on the brick that the selected source for selected_source + * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417 + * @param throttle_normalized Throttle of the vehicle, between 0 and 1 + * @param armed Arming state of the vehicle + */ + void updateBatteryStatusRawADC(int32_t voltage_raw, int32_t current_raw, hrt_abstime timestamp, + bool selected_source, int priority, float throttle_normalized, bool armed); + + /** + * Update current battery status message. + * + * @param voltage_raw Battery voltage read from ADC, in Volts + * @param current_raw Voltage of current sense resistor, in Amps + * @param timestamp Time at which the ADC was read (use hrt_absolute_time()) + * @param selected_source This battery is on the brick that the selected source for selected_source + * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417 + * @param throttle_normalized Throttle of the vehicle, between 0 and 1 + * @param armed Arming state of the vehicle + */ + void updateBatteryStatus(float voltage_v, float current_a, hrt_abstime timestamp, + bool selected_source, int priority, float throttle_normalized, bool armed); + + /** + * Which ADC channel is used for voltage reading of this battery + */ + int vChannel{-1}; + /** + * Which ADC channel is used for current reading of this battery + */ + int iChannel{-1}; + + /** + * Whether the ADC channel for the voltage of this battery is valid. + * Corresponds to BOARD_BRICK_VALID_LIST + */ + bool is_valid() + { +#ifdef BOARD_BRICK_VALID_LIST + bool valid[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST; + return valid[_get_brick_index()]; +#else + return true; +#endif + } + +protected: + // Defaults to use if the parameters are not set +#if BOARD_NUMBER_BRICKS > 0 +#if defined(BOARD_BATT_V_LIST) && defined(BOARD_BATT_I_LIST) + static constexpr int DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST; + static constexpr int DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST; +#else + static constexpr int DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = {0}; + static constexpr int DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = {0}; +#endif +#else + static constexpr int DEFAULT_V_CHANNEL[0] = {}; + static constexpr int DEFAULT_I_CHANNEL[0] = {}; +#endif + + // The following are all of the parameters needed for the batteries. + // See battery.h for example implementation. + virtual float _get_bat_v_empty() = 0; + virtual float _get_bat_v_charged() = 0; + virtual int _get_bat_n_cells() = 0; + virtual float _get_bat_capacity() = 0; + virtual float _get_bat_v_load_drop() = 0; + virtual float _get_bat_r_internal() = 0; + virtual float _get_bat_low_thr() = 0; + virtual float _get_bat_crit_thr() = 0; + virtual float _get_bat_emergen_thr() = 0; + virtual float _get_cnt_v_volt_raw() = 0; + virtual float _get_cnt_v_curr_raw() = 0; + virtual float _get_v_offs_cur() = 0; + virtual float _get_v_div_raw() = 0; + virtual float _get_a_per_v_raw() = 0; + virtual int _get_source() = 0; + virtual int _get_adc_channel() = 0; + + virtual int _get_brick_index() = 0; + + float _get_cnt_v_volt(); + float _get_cnt_v_curr(); + float _get_v_div(); + float _get_a_per_v(); + +private: + void filterVoltage(float voltage_v); + void filterThrottle(float throttle); + void filterCurrent(float current_a); + void sumDischarged(hrt_abstime timestamp, float current_a); + void estimateRemaining(float voltage_v, float current_a, float throttle, bool armed); + void determineWarning(bool connected); + void computeScale(); + + bool _battery_initialized = false; + float _voltage_filtered_v = -1.f; + float _throttle_filtered = -1.f; + float _current_filtered_a = -1.f; + float _discharged_mah = 0.f; + float _discharged_mah_loop = 0.f; + float _remaining_voltage = -1.f; ///< normalized battery charge level remaining based on voltage + float _remaining = -1.f; ///< normalized battery charge level, selected based on config param + float _scale = 1.f; + uint8_t _warning; + hrt_abstime _last_timestamp; + + orb_advert_t _orbAdvert{nullptr}; + int _orbInstance; + battery_status_s _battery_status; +}; diff --git a/src/lib/battery/battery_params.c b/src/lib/battery/battery_params_1.c similarity index 72% rename from src/lib/battery/battery_params.c rename to src/lib/battery/battery_params_1.c index 9a51cf2806..4c0b35632d 100644 --- a/src/lib/battery/battery_params.c +++ b/src/lib/battery/battery_params_1.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,11 +32,11 @@ ****************************************************************************/ /** - * @file battery_params.c + * @file battery_params_1.c + * @author Timothy Scott * - * Parameters defined by the battery lib. - * - * @author Julian Oes + * Defines parameters for Battery 1. For backwards compatibility, the + * parameter names do not have a "1" in them. */ #include @@ -45,7 +45,7 @@ /** * Empty cell voltage (5C load) * - * Defines the voltage where a single cell of the battery is considered empty. + * Defines the voltage where a single cell of battery 1 is considered empty. * The voltage should be chosen before the steep dropoff to 2.8V. A typical * lithium battery can only be discharged down to 10% before it drops off * to a voltage level damaging the cells. @@ -61,7 +61,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.5f); /** * Full cell voltage (5C load) * - * Defines the voltage where a single cell of the battery is considered full + * Defines the voltage where a single cell of battery 1 is considered full * under a mild load. This will never be the nominal voltage of 4.2V * * @group Battery Calibration @@ -72,61 +72,11 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.5f); */ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.05f); -/** - * Low threshold - * - * Sets the threshold when the battery will be reported as low. - * This has to be higher than the critical threshold. - * - * @group Battery Calibration - * @unit norm - * @min 0.12 - * @max 0.4 - * @decimal 2 - * @increment 0.01 - * @reboot_required true - */ -PARAM_DEFINE_FLOAT(BAT_LOW_THR, 0.15f); - -/** - * Critical threshold - * - * Sets the threshold when the battery will be reported as critically low. - * This has to be lower than the low threshold. This threshold commonly - * will trigger RTL. - * - * @group Battery Calibration - * @unit norm - * @min 0.05 - * @max 0.1 - * @decimal 2 - * @increment 0.01 - * @reboot_required true - */ -PARAM_DEFINE_FLOAT(BAT_CRIT_THR, 0.07f); - -/** - * Emergency threshold - * - * Sets the threshold when the battery will be reported as dangerously low. - * This has to be lower than the critical threshold. This threshold commonly - * will trigger landing. - * - * @group Battery Calibration - * @unit norm - * @min 0.03 - * @max 0.07 - * @decimal 2 - * @increment 0.01 - * @reboot_required true - */ -PARAM_DEFINE_FLOAT(BAT_EMERGEN_THR, 0.05f); - /** * Voltage drop per cell on full throttle * * This implicitely defines the internal resistance - * to maximum current ratio and assumes linearity. + * to maximum current ratio for battery 1 and assumes linearity. * A good value to use is the difference between the * 5C and 20-25C load. Not used if BAT_R_INTERNAL is * set. @@ -142,7 +92,7 @@ PARAM_DEFINE_FLOAT(BAT_EMERGEN_THR, 0.05f); PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f); /** - * Explicitly defines the per cell internal resistance + * Explicitly defines the per cell internal resistance for battery 1 * * If non-negative, then this will be used in place of * BAT_V_LOAD_DROP for all calculations. @@ -155,8 +105,9 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f); */ PARAM_DEFINE_FLOAT(BAT_R_INTERNAL, -1.0f); + /** - * Number of cells. + * Number of cells for battery 1. * * Defines the number of cells the attached battery consists of. * @@ -183,9 +134,9 @@ PARAM_DEFINE_FLOAT(BAT_R_INTERNAL, -1.0f); PARAM_DEFINE_INT32(BAT_N_CELLS, 0); /** - * Battery capacity. + * Battery 1 capacity. * - * Defines the capacity of the attached battery. + * Defines the capacity of battery 1. * * @group Battery Calibration * @unit mAh @@ -196,3 +147,38 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 0); * @reboot_required true */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); + +/** + * Battery 1 voltage divider (V divider) + * + * This is the divider from battery 1 voltage to 3.3V ADC voltage. + * If using e.g. Mauch power modules the value from the datasheet + * can be applied straight here. A value of -1 means to use + * the board default. + * + * @group Battery Calibration + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0); + +/** + * Battery 1 current per volt (A/V) + * + * The voltage seen by the 3.3V ADC multiplied by this factor + * will determine the battery current. A value of -1 means to use + * the board default. + * + * @group Battery Calibration + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0); + +/** + * Battery 1 ADC Channel + * + * This parameter specifies the ADC channel used to monitor voltage of main power battery. + * A value of -1 means to use the board default. + * + * @group Battery Calibration + */ +PARAM_DEFINE_INT32(BAT_ADC_CHANNEL, -1); \ No newline at end of file diff --git a/src/lib/battery/battery_params_2.c b/src/lib/battery/battery_params_2.c new file mode 100644 index 0000000000..4fb1eadd2b --- /dev/null +++ b/src/lib/battery/battery_params_2.c @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file battery_params_2.c + * @author Timothy Scott + * + * Defines parameters for Battery 2. + */ + +/** + * Empty cell voltage (5C load) + * + * Defines the voltage where a single cell of battery 2 is considered empty. + * The voltage should be chosen before the steep dropoff to 2.8V. A typical + * lithium battery can only be discharged down to 10% before it drops off + * to a voltage level damaging the cells. + * + * @group Battery Calibration + * @unit V + * @decimal 2 + * @increment 0.01 + * @reboot_required true + */ +PARAM_DEFINE_FLOAT(BAT2_V_EMPTY, 3.5f); + +/** + * Full cell voltage (5C load) + * + * Defines the voltage where a single cell of battery 2 is considered full + * under a mild load. This will never be the nominal voltage of 4.2V + * + * @group Battery Calibration + * @unit V + * @decimal 2 + * @increment 0.01 + * @reboot_required true + */ +PARAM_DEFINE_FLOAT(BAT2_V_CHARGED, 4.05f); + +/** + * Voltage drop per cell on full throttle + * + * This implicitely defines the internal resistance + * to maximum current ratio for battery 2 and assumes linearity. + * A good value to use is the difference between the + * 5C and 20-25C load. Not used if BAT2_R_INTERNAL is + * set. + * + * @group Battery Calibration + * @unit V + * @min 0.07 + * @max 0.5 + * @decimal 2 + * @increment 0.01 + * @reboot_required true + */ +PARAM_DEFINE_FLOAT(BAT2_V_LOAD_DROP, 0.3f); + +/** + * Explicitly defines the per cell internal resistance for battery 2 + * + * If non-negative, then this will be used in place of + * BAT2_V_LOAD_DROP for all calculations. + * + * @group Battery Calibration + * @unit Ohms + * @min -1.0 + * @max 0.2 + * @reboot_required true + */ +PARAM_DEFINE_FLOAT(BAT2_R_INTERNAL, -1.0f); + + +/** + * Number of cells for battery 2. + * + * Defines the number of cells the attached battery consists of. + * + * @group Battery Calibration + * @unit S + * @value 0 Unconfigured + * @value 2 2S Battery + * @value 3 3S Battery + * @value 4 4S Battery + * @value 5 5S Battery + * @value 6 6S Battery + * @value 7 7S Battery + * @value 8 8S Battery + * @value 9 9S Battery + * @value 10 10S Battery + * @value 11 11S Battery + * @value 12 12S Battery + * @value 13 13S Battery + * @value 14 14S Battery + * @value 15 15S Battery + * @value 16 16S Battery + * @reboot_required true + */ +PARAM_DEFINE_INT32(BAT2_N_CELLS, 0); + +/** + * Battery 2 capacity. + * + * Defines the capacity of battery 2. + * + * @group Battery Calibration + * @unit mAh + * @decimal 0 + * @min -1.0 + * @max 100000 + * @increment 50 + * @reboot_required true + */ +PARAM_DEFINE_FLOAT(BAT2_CAPACITY, -1.0f); + +/** + * Battery 2 voltage divider (V divider) + * + * This is the divider from battery 2 voltage to 3.3V ADC voltage. + * If using e.g. Mauch power modules the value from the datasheet + * can be applied straight here. A value of -1 means to use + * the board default. + * + * @group Battery Calibration + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(BAT2_V_DIV, -1.0); + +/** + * Battery 2 current per volt (A/V) + * + * The voltage seen by the 3.3V ADC multiplied by this factor + * will determine the battery current. A value of -1 means to use + * the board default. + * + * @group Battery Calibration + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(BAT2_A_PER_V, -1.0); + +/** + * Battery 2 ADC Channel + * + * This parameter specifies the ADC channel used to monitor voltage of main power battery. + * A value of -1 means to use the board default. + * + * @group Battery Calibration + */ +PARAM_DEFINE_INT32(BAT2_ADC_CHANNEL, -1); \ No newline at end of file diff --git a/src/lib/battery/battery_params_common.c b/src/lib/battery/battery_params_common.c new file mode 100644 index 0000000000..8883d8796b --- /dev/null +++ b/src/lib/battery/battery_params_common.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file battery_params.c + * + * Parameters defined by the battery lib, shared between all batteries. + * + * @author Julian Oes + */ + +#include +#include + + +/** + * Low threshold + * + * Sets the threshold when the battery will be reported as low. + * This has to be higher than the critical threshold. + * + * @group Battery Calibration + * @unit norm + * @min 0.12 + * @max 0.4 + * @decimal 2 + * @increment 0.01 + * @reboot_required true + */ +PARAM_DEFINE_FLOAT(BAT_LOW_THR, 0.15f); + +/** + * Critical threshold + * + * Sets the threshold when the battery will be reported as critically low. + * This has to be lower than the low threshold. This threshold commonly + * will trigger RTL. + * + * @group Battery Calibration + * @unit norm + * @min 0.05 + * @max 0.1 + * @decimal 2 + * @increment 0.01 + * @reboot_required true + */ +PARAM_DEFINE_FLOAT(BAT_CRIT_THR, 0.07f); + +/** + * Emergency threshold + * + * Sets the threshold when the battery will be reported as dangerously low. + * This has to be lower than the critical threshold. This threshold commonly + * will trigger landing. + * + * @group Battery Calibration + * @unit norm + * @min 0.03 + * @max 0.07 + * @decimal 2 + * @increment 0.01 + * @reboot_required true + */ +PARAM_DEFINE_FLOAT(BAT_EMERGEN_THR, 0.05f); + +/** + * Scaling from ADC counts to volt on the ADC input (battery voltage) + * + * This is not the battery voltage, but the intermediate ADC voltage. + * A value of -1 signifies that the board defaults are used, which is + * highly recommended. + * + * @group Battery Calibration + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(BAT_CNT_V_VOLT, -1.0f); + +/** + * Scaling from ADC counts to volt on the ADC input (battery current) + * + * This is not the battery current, but the intermediate ADC voltage. + * A value of -1 signifies that the board defaults are used, which is + * highly recommended. + * + * @group Battery Calibration + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(BAT_CNT_V_CURR, -1.0); + +/** + * Offset in volt as seen by the ADC input of the current sensor. + * + * This offset will be subtracted before calculating the battery + * current based on the voltage. + * + * @group Battery Calibration + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(BAT_V_OFFS_CURR, 0.0); + +/** + * Battery monitoring source. + * + * This parameter controls the source of battery data. The value 'Power Module' + * means that measurements are expected to come from a power module. If the value is set to + * 'External' then the system expects to receive mavlink battery status messages. + * + * @min 0 + * @max 1 + * @value 0 Power Module + * @value 1 External + * @group Battery Calibration + */ +PARAM_DEFINE_INT32(BAT_SOURCE, 0); diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 177f7e38e0..aa539be66a 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -43,6 +43,7 @@ px4_add_module( sensors.cpp parameters.cpp temperature_compensation.cpp + power.cpp DEPENDS airspeed diff --git a/src/modules/sensors/power.cpp b/src/modules/sensors/power.cpp new file mode 100644 index 0000000000..c80f52497a --- /dev/null +++ b/src/modules/sensors/power.cpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file power.cpp + * + * @author Timothy Scott + */ +#include "power.h" + +Power::Power() {} + +void Power::update(px4_adc_msg_t buf_adc[PX4_MAX_ADC_CHANNELS], int nchannels, float throttle, bool armed) +{ + +#if BOARD_NUMBER_BRICKS > 0 + /* For legacy support we publish the battery_status for the Battery that is + * associated with the Brick that is the selected source for VDD_5V_IN + * Selection is done in HW ala a LTC4417 or similar, or may be hard coded + * Like in the FMUv4 + */ + + /* Per Brick readings with default unread channels at 0 */ + int32_t bat_current_cnt[BOARD_NUMBER_BRICKS] = {0}; + int32_t bat_voltage_cnt[BOARD_NUMBER_BRICKS] = {0}; + + // The channel readings are not necessarily in a nice order, so we iterate through + // to find every relevant channel. + for (int i = 0; i < nchannels; i++) { + for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) { + /* look for specific channels and process the raw voltage to measurement data */ + if (_analogBatteries[b]->vChannel == buf_adc[i].am_channel) { + /* Voltage in ADC counts */ + bat_voltage_cnt[b] = buf_adc[i].am_data; + + } else if (_analogBatteries[b]->iChannel == buf_adc[i].am_channel) { + /* Voltage at current sense resistor in ADC counts */ + bat_current_cnt[b] = buf_adc[i].am_data; + } + } + } + + /* Based on the valid_chan, used to indicate the selected the lowest index + * (highest priority) supply that is the source for the VDD_5V_IN + * When < 0 none selected + */ + + int selected_source = -1; + + for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) { + if (_analogBatteries[b]->is_valid() && selected_source < 0) { + selected_source = b; + } + + _analogBatteries[b]->updateBatteryStatusRawADC(bat_voltage_cnt[b], bat_current_cnt[b], hrt_absolute_time(), + selected_source == b, b, throttle, armed); + } + +#endif /* BOARD_NUMBER_BRICKS > 0 */ + +} \ No newline at end of file diff --git a/src/modules/sensors/power.h b/src/modules/sensors/power.h new file mode 100644 index 0000000000..1f47441e01 --- /dev/null +++ b/src/modules/sensors/power.h @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file power.h + * + * @author Timothy Scott + */ + +#pragma once + +#include +#include + +#ifdef BOARD_NUMBER_DIGITAL_BRICKS +#define TOTAL_BRICKS (BOARD_NUMBER_BRICKS + BOARD_NUMBER_DIGITAL_BRICKS) +#else +#define TOTAL_BRICKS BOARD_NUMBER_BRICKS +#endif + +/** + * Measures voltage, current, etc. of all batteries connected to the vehicle, both + * analog and digital. + */ +class Power +{ +public: + Power(); + + /** + * Updates the measurements of each battery. + * + * If the parameter `BAT_SOURCE` == 0, this function will also publish an instance of the uORB topic + * `battery_status` for each battery. For reasons of backwards compability, instance 0 will always be the + * primary battery. However, this may change in the future! In the future, please use orb_priority() to find + * the primary battery. + * @param buf_adc Buffer of ADC readings + * @param nchannels Number of valid ADC readings in `buf_adc` + * @param throttle Normalized throttle (between 0 and 1, or maybe between -1 and 1 in the future) + * @param armed True if the vehicle is armed + */ + void update(px4_adc_msg_t buf_adc[PX4_MAX_ADC_CHANNELS], int nchannels, float throttle, bool armed); + +private: + + /* + * All of these #if's are doing one thing: Building an array of `BatteryBase` objects, one + * for each possible connected battery. A `BatteryBase` object does not mean that a battery IS connected, + * it just means that one CAN be connected. + * + * For an example of what this looks like after preprocessing, assume that BOARD_NUMBER_BRICKS = 2: + * ``` + * Battery1 _battery0; + * Battery2 _battery1; + * + * BatteryBase *_analogBatteries[2] { + * &_battery0, + * &_battery1, + * } + * ``` + * + * The #if BOARD_NUMBER_BRICKS > 0 wraps the entire declaration because otherwise, an empty array is declared + * which then is unused. In some configurations, an unused variable throws a compile error. + */ + + // TODO: Add digital batteries + +#if BOARD_NUMBER_BRICKS > 0 + Battery1 _battery0; + +#if BOARD_NUMBER_BRICKS > 1 + Battery2 _battery1; +#endif + + BatteryBase *_analogBatteries[BOARD_NUMBER_BRICKS] { + &_battery0, +#if BOARD_NUMBER_BRICKS > 1 + &_battery1, +#endif + }; // End _analogBatteries +#endif // End #if BOARD_NUMBER_BRICKS > 0 +}; diff --git a/src/modules/sensors/sensor_params_battery.c b/src/modules/sensors/sensor_params_battery.c deleted file mode 100644 index 47f2237f9b..0000000000 --- a/src/modules/sensors/sensor_params_battery.c +++ /dev/null @@ -1,57 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Battery voltage divider (V divider) - * - * This is the divider from battery voltage to 3.3V ADC voltage. - * If using e.g. Mauch power modules the value from the datasheet - * can be applied straight here. A value of -1 means to use - * the board default. - * - * @group Battery Calibration - * @decimal 8 - */ -PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0); - -/** - * Battery current per volt (A/V) - * - * The voltage seen by the 3.3V ADC multiplied by this factor - * will determine the battery current. A value of -1 means to use - * the board default. - * - * @group Battery Calibration - * @decimal 8 - */ -PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 105d0f5f36..5d1ed7b249 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -142,7 +142,6 @@ private: VotedSensorsUpdate _voted_sensors_update; - VehicleAcceleration _vehicle_acceleration; VehicleAngularVelocity _vehicle_angular_velocity; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 0ca878417a..bc9db1a273 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -239,7 +239,7 @@ private: hrt_abstime _last_sitl_timestamp{0}; // Lib used to do the battery calculations. - Battery _battery {}; + Battery1 _battery {}; battery_status_s _battery_status{}; #ifndef __PX4_QURT diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 73defef146..8b54244428 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -370,10 +370,7 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) vbatt *= _battery.cell_count(); const float throttle = 0.0f; // simulate no throttle compensation to make the estimate predictable - _battery.updateBatteryStatus(now_us, vbatt, ibatt, true, true, 0, throttle, armed, &_battery_status); - - // publish the battery voltage - _battery_pub.publish(_battery_status); + _battery.updateBatteryStatus(vbatt, ibatt, now_us, true, 0, throttle, armed); } }