diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index daddf1f70b..682c9305c6 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp @@ -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(vbat, -1, t, true, 0, 0, false); + _battery.updateBatteryStatus(t, vbat, -1, true, true, 0, 0, false, true); // Update battery charge state 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 0d54d2e38d..7b3597999f 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 @@ -220,7 +220,7 @@ int DfBebopBusWrapper::_publish(struct bebop_state_data &data) // 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); + _battery.updateBatteryStatus(timestamp, data.battery_voltage_v, 0.0, true, true, 0, _last_throttle, _armed, true); 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 3428f6112c..15aa9674d0 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 @@ -131,16 +131,17 @@ 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(data.battery_voltage_V, data.battery_current_A, t, - true, 1, + _battery.updateBatteryStatus(t, data.battery_voltage_V, data.battery_current_A, + connected, true, 1, ctrl.control[actuator_controls_s::INDEX_THROTTLE], - vcontrol_mode.flag_armed); + vcontrol_mode.flag_armed, true); return 0; } diff --git a/src/lib/battery/CMakeLists.txt b/src/lib/battery/CMakeLists.txt index de7ac02ab2..698be14941 100644 --- a/src/lib/battery/CMakeLists.txt +++ b/src/lib/battery/CMakeLists.txt @@ -31,4 +31,4 @@ # ############################################################################ -px4_add_library(battery battery_base.cpp) +px4_add_library(battery battery_base.cpp battery.cpp) diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp new file mode 100644 index 0000000000..80569edd3a --- /dev/null +++ b/src/lib/battery/battery.cpp @@ -0,0 +1,14 @@ + +#include "battery.h" + +Battery1::Battery1() +{ + updateParams(); + + migrateParam(_param_old_bat_v_empty, _param_bat_v_empty, "V_EMPTY", 3.5f); + migrateParam(_param_old_bat_v_charged, _param_bat_v_charged, "V_CHARGED", 4.05f); + migrateParam(_param_old_bat_v_load_drop, _param_bat_v_load_drop, "V_LOAD_DROP", 0.3f); + migrateParam(_param_old_bat_r_internal, _param_bat_r_internal, "R_INTERNAL", -1.0f); + migrateParam(_param_old_bat_n_cells, _param_bat_n_cells, "N_CELLS", 0); + migrateParam(_param_old_bat_capacity, _param_bat_capacity, "CAPACITY", -1.0f); +} diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index bda9f09d88..1ef5ee13d8 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -33,12 +33,18 @@ #pragma once +#include "battery_base.h" + +#include +#include + /** * @file battery.h - * Implementations of BatteryBase + * Basic implementation of BatteryBase. Battery1 is calibrated by BAT1_* parameters. Battery2 is calibrated + * by BAT2_* parameters. * * 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. + * (Battery 1 uses `BAT1_*`, 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. * @@ -47,54 +53,89 @@ * 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. + * To add a third battery, follow these steps: + * - Copy/Paste all of Battery2 to make Battery3 + * - Change all "BAT2_*" parameters to "BAT3_*" in Battery3 + * - Copy the file "battery_params_2.c" to "battery_params_3.c" + * - Change all of the "BAT2_*" params in "battery_params_3.c" to "BAT3_*" + * This is not done now because there is not yet any demand for a third battery, and adding parameters uses up space. */ -#include "battery_base.h" - +/** + * Battery1 represents a battery calibrated by BAT1_* parameters. + */ class Battery1 : public BatteryBase { public: - Battery1() : BatteryBase() + Battery1(); + + /** + * This function migrates the old deprecated parameters like BAT_N_CELLS to the new parameters like BAT1_N_CELLS. + * It checks if the old parameter is non-defaulT AND the new parameter IS default, and if so, it: + * - Issues a warning using PX4_WARN(...) + * - Copies the value of the old parameter over to the new parameter + * - Resets the old parameter to its default + * + * The 'name' parameter should be only the part of the parameter name that comes after "BAT1_" or "BAT_". It is + * used only for the warning message. For example, for parameter BAT1_N_CELLS, name should be "N_CELLS". + * (See the implementation of this function for why I have taken this strange choice) + * + * In an ideal world, this function would be protected so that only child classes of Battery1 could access it. + * However, the way ModuleParams works makes it very difficult to inherit from a ModuleParams class. + * For example, the AnalogBattery class in the Sensors module does not inherit this class; it just contains + * a Battery1 member variable. + * + * The templating is complicated because every parameter is technically a different type. However, in normal + * use, the template can just be ignored. See the implementation of Battery1::Battery1() for example usage. + * + * @tparam P1 Type of the first parameter + * @tparam P2 Type of the second parameter + * @tparam T Data type for the default value + * @param oldParam Reference to the old parameter, as a ParamFloat<...>, ParamInt<...>, or ParamBool<...> + * @param newParam Reference to the new paramater, as a ParamFloat<...>, ParamInt<...>, or ParamBool<...> + * @param name The name of the parameter, WITHOUT the "BAT_" or "BAT1_" prefix. This is used only for logging. + * @param defaultValue Default value of the parameter, as specified in PARAM_DEFINE_*(...) + */ + template static void + migrateParam(P1 &oldParam, P2 &newParam, const char *name, T defaultValue) { - // Can't do this in the constructor because virtual functions - if (_get_adc_channel() >= 0) { - vChannel = _get_adc_channel(); + float diffOld = fabs((float) oldParam.get() - defaultValue); + float diffNew = fabs((float) newParam.get() - defaultValue); - } else { - vChannel = DEFAULT_V_CHANNEL[0]; + if (diffOld > 0.0001f && diffNew < 0.0001f) { + PX4_WARN("Parameter BAT_%s is deprecated. Copying value to BAT1_%s.", name, name); + newParam.set(oldParam.get()); + oldParam.set(defaultValue); + newParam.commit(); + oldParam.commit(); } - - // TODO: Add parameter, like with V - iChannel = DEFAULT_I_CHANNEL[0]; } 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_old_bat_v_empty, + (ParamFloat) _param_old_bat_v_charged, + (ParamInt) _param_old_bat_n_cells, + (ParamFloat) _param_old_bat_capacity, + (ParamFloat) _param_old_bat_v_load_drop, + (ParamFloat) _param_old_bat_r_internal, + + (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_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 ) @@ -107,34 +148,16 @@ private: 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; } }; +/** + * Battery2 represents a battery calibrated by BAT2_* parameters. + */ 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]; - } - + Battery2() {} private: DEFINE_PARAMETERS( @@ -144,16 +167,10 @@ 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_cnt_v_volt, - (ParamFloat) _param_cnt_v_curr, - (ParamFloat) _param_v_offs_cur, (ParamInt) _param_source ) @@ -166,13 +183,5 @@ private: 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; } -}; +}; \ No newline at end of file diff --git a/src/lib/battery/battery_base.cpp b/src/lib/battery/battery_base.cpp index 02ea25396e..7a6c382449 100644 --- a/src/lib/battery/battery_base.cpp +++ b/src/lib/battery/battery_base.cpp @@ -63,32 +63,13 @@ BatteryBase::reset() // 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.capacity = _get_bat_capacity(); } void -BatteryBase::updateBatteryStatusRawADC(int32_t voltage_raw, int32_t current_raw, hrt_abstime timestamp, - bool selected_source, int priority, - float throttle_normalized, - bool armed) +BatteryBase::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, bool connected, + bool selected_source, int priority, float throttle_normalized, bool armed, bool should_publish) { - - 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); @@ -119,11 +100,17 @@ BatteryBase::updateBatteryStatus(float voltage_v, float current_a, hrt_abstime t _battery_status.timestamp = timestamp; - if (_get_source() == 0) { - orb_publish_auto(ORB_ID(battery_status), &_orbAdvert, &_battery_status, &_orbInstance, ORB_PRIO_DEFAULT); + if (should_publish) { + publish(); } - battery_status->temperature = NAN; + _battery_status.temperature = NAN; +} + +void +BatteryBase::publish() +{ + orb_publish_auto(ORB_ID(battery_status), &_orbAdvert, &_battery_status, &_orbInstance, ORB_PRIO_DEFAULT); } void @@ -267,55 +254,3 @@ BatteryBase::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 index 1eb2bc9e4f..a636efda9d 100644 --- a/src/lib/battery/battery_base.h +++ b/src/lib/battery/battery_base.h @@ -53,7 +53,15 @@ /** * 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. + * Each of the virtual _get_*() functions corresponds to a parameter used for calibration. However, depending + * on the type of battery, these values may not come from parameters. + * + * Most implementations of BatteryBase will also inherit ModuleParams, and most of the virtual function + * implementations will look something like: + * float _get_bat_v_empty() override {return _param_bat_v_empty().get();} + * + * For a full implementation, see src/modules/sensors/analog_battery.{cpp, h} + * For a minimal implementation, see src/modules/simulator/simulator.h */ class BatteryBase : ModuleParams { @@ -80,20 +88,6 @@ public: */ 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. * @@ -104,73 +98,64 @@ public: * @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 + * @param should_publish If True, this function published a battery_status uORB message. */ - void updateBatteryStatus(float voltage_v, float current_a, hrt_abstime timestamp, - bool selected_source, int priority, float throttle_normalized, bool armed); + void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, bool connected, + bool selected_source, int priority, float throttle_normalized, bool armed, bool should_publish); /** - * Which ADC channel is used for voltage reading of this battery + * Publishes the uORB battery_status message with the most recently-updated data. */ - int vChannel{-1}; - /** - * Which ADC channel is used for current reading of this battery - */ - int iChannel{-1}; + void publish(); - /** - * 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 - } + // TODO: Check that there was actually a parameter update + void updateParams() {ModuleParams::updateParams();} 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. + /** + * @return Value, in volts, at which a single cell of the battery would be considered empty. + */ virtual float _get_bat_v_empty() = 0; + /** + * @return Value, in volts, at which a single cell of the battery would be considered full. + */ virtual float _get_bat_v_charged() = 0; + /** + * @return Number of cells in series in the battery. + */ virtual int _get_bat_n_cells() = 0; + /** + * @return Total capacity of the battery, in mAh + */ virtual float _get_bat_capacity() = 0; + /** + * @return Voltage drop per cell at full throttle. This implicitly defines the internal resistance of the + * battery. If _get_bat_r_internal() >= to 0, then _get_bat_v_load_drop() is ignored. + */ virtual float _get_bat_v_load_drop() = 0; + /** + * @return Internal resistance of the battery, in Ohms. If non-negative, then this is used instead of + * _get_bat_v_load_drop(). + */ virtual float _get_bat_r_internal() = 0; + /** + * @return Low battery threshold, as fraction between 0 and 1. + */ virtual float _get_bat_low_thr() = 0; + /** + * @return Critical battery threshold, as fraction between 0 and 1. + */ virtual float _get_bat_crit_thr() = 0; + /** + * @return Emergency battery threshold, as fraction between 0 and 1. + */ 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; + /** + * @return Source of battery data. 0 = internal power module. 1 = external, via Mavlink + */ 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(); + battery_status_s _battery_status; private: void filterVoltage(float voltage_v); @@ -195,5 +180,4 @@ private: orb_advert_t _orbAdvert{nullptr}; int _orbInstance; - battery_status_s _battery_status; }; diff --git a/src/lib/battery/battery_params_1.c b/src/lib/battery/battery_params_1.c index 4c0b35632d..fed4575afa 100644 --- a/src/lib/battery/battery_params_1.c +++ b/src/lib/battery/battery_params_1.c @@ -56,7 +56,7 @@ * @increment 0.01 * @reboot_required true */ -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.5f); +PARAM_DEFINE_FLOAT(BAT1_V_EMPTY, 3.5f); /** * Full cell voltage (5C load) @@ -70,7 +70,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.5f); * @increment 0.01 * @reboot_required true */ -PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.05f); +PARAM_DEFINE_FLOAT(BAT1_V_CHARGED, 4.05f); /** * Voltage drop per cell on full throttle @@ -78,7 +78,7 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.05f); * This implicitely defines the internal resistance * 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 + * 5C and 20-25C load. Not used if BAT1_R_INTERNAL is * set. * * @group Battery Calibration @@ -89,13 +89,13 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.05f); * @increment 0.01 * @reboot_required true */ -PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f); +PARAM_DEFINE_FLOAT(BAT1_V_LOAD_DROP, 0.3f); /** * 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. + * BAT1_V_LOAD_DROP for all calculations. * * @group Battery Calibration * @unit Ohms @@ -103,7 +103,7 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f); * @max 0.2 * @reboot_required true */ -PARAM_DEFINE_FLOAT(BAT_R_INTERNAL, -1.0f); +PARAM_DEFINE_FLOAT(BAT1_R_INTERNAL, -1.0f); /** @@ -131,7 +131,7 @@ PARAM_DEFINE_FLOAT(BAT_R_INTERNAL, -1.0f); * @value 16 16S Battery * @reboot_required true */ -PARAM_DEFINE_INT32(BAT_N_CELLS, 0); +PARAM_DEFINE_INT32(BAT1_N_CELLS, 0); /** * Battery 1 capacity. @@ -146,7 +146,7 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 0); * @increment 50 * @reboot_required true */ -PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); +PARAM_DEFINE_FLOAT(BAT1_CAPACITY, -1.0f); /** * Battery 1 voltage divider (V divider) @@ -159,7 +159,7 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); * @group Battery Calibration * @decimal 8 */ -PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0); +PARAM_DEFINE_FLOAT(BAT1_V_DIV, -1.0); /** * Battery 1 current per volt (A/V) @@ -171,7 +171,7 @@ PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0); * @group Battery Calibration * @decimal 8 */ -PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0); +PARAM_DEFINE_FLOAT(BAT1_A_PER_V, -1.0); /** * Battery 1 ADC Channel @@ -181,4 +181,4 @@ PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0); * * @group Battery Calibration */ -PARAM_DEFINE_INT32(BAT_ADC_CHANNEL, -1); \ No newline at end of file +PARAM_DEFINE_INT32(BAT1_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 index 4fb1eadd2b..5178d6c565 100644 --- a/src/lib/battery/battery_params_2.c +++ b/src/lib/battery/battery_params_2.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. + * 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 diff --git a/src/lib/battery/battery_params_common.c b/src/lib/battery/battery_params_common.c index 8883d8796b..1def2d99d4 100644 --- a/src/lib/battery/battery_params_common.c +++ b/src/lib/battery/battery_params_common.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 diff --git a/src/lib/battery/battery_params_deprecated.c b/src/lib/battery/battery_params_deprecated.c new file mode 100644 index 0000000000..1ada9ef2f1 --- /dev/null +++ b/src/lib/battery/battery_params_deprecated.c @@ -0,0 +1,181 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file battery_params_1.c + * @author Timothy Scott + * + * Defines parameters for Battery 1. For backwards compatibility, the + * parameter names do not have a "1" in them. + */ + +/** + * This parameter is deprecated. Please use BAT1_V_EMPTY instead. + * + * 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. + * + * @group Battery Calibration + * @unit V + * @decimal 2 + * @increment 0.01 + * @reboot_required true + */ +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.5f); + +/** + * This parameter is deprecated. Please use BAT1_V_CHARGED instead. + * + * 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 + * @unit V + * @decimal 2 + * @increment 0.01 + * @reboot_required true + */ +PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.05f); + +/** + * This parameter is deprecated. Please use BAT1_V_LOAD_DROP instead. + * + * This implicitely defines the internal resistance + * 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. + * + * @group Battery Calibration + * @unit V + * @min 0.07 + * @max 0.5 + * @decimal 2 + * @increment 0.01 + * @reboot_required true + */ +PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f); + +/** + * This parameter is deprecated. Please use BAT1_R_INTERNAL instead. + * + * If non-negative, then this will be used in place of + * BAT_V_LOAD_DROP for all calculations. + * + * @group Battery Calibration + * @unit Ohms + * @min -1.0 + * @max 0.2 + * @reboot_required true + */ +PARAM_DEFINE_FLOAT(BAT_R_INTERNAL, -1.0f); + + +/** + * This parameter is deprecated. Please use BAT1_N_CELLS instead. + * + * 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(BAT_N_CELLS, 0); + +/** + * This parameter is deprecated. Please use BAT1_CAPACITY instead. + * + * Defines the capacity of battery 1. + * + * @group Battery Calibration + * @unit mAh + * @decimal 0 + * @min -1.0 + * @max 100000 + * @increment 50 + * @reboot_required true + */ +PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); + +/** + * This parameter is deprecated. Please use BAT1_V_DIV instead. + * + * 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); + +/** + * This parameter is deprecated. Please use BAT1_A_PER_V instead. + * + * 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); + +/** + * This parameter is deprecated. Please use BAT1_ADC_CHANNEL instead. + * + * 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/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index aa539be66a..f3cfa44f97 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -44,6 +44,7 @@ px4_add_module( parameters.cpp temperature_compensation.cpp power.cpp + analog_battery.cpp DEPENDS airspeed diff --git a/src/modules/sensors/analog_battery.cpp b/src/modules/sensors/analog_battery.cpp new file mode 100644 index 0000000000..85ca2ae811 --- /dev/null +++ b/src/modules/sensors/analog_battery.cpp @@ -0,0 +1,141 @@ +#include "analog_battery.h" + +// 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[1] = {0}; +static constexpr int DEFAULT_I_CHANNEL[1] = {0}; +#endif + +static constexpr float DEFAULT_VOLTS_PER_COUNT = 3.3f / 4096.0f; + +AnalogBattery::AnalogBattery() : + ModuleParams(nullptr) +{} + +void +AnalogBattery::updateBatteryStatusRawADC(hrt_abstime timestamp, int32_t voltage_raw, int32_t current_raw, + bool selected_source, int priority, float throttle_normalized, + bool armed) +{ + // TODO: Check that there was actually a parameter update + _get_battery_base().updateParams(); + updateParams(); + + 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(); + + bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V && + (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid()); + + _get_battery_base().updateBatteryStatus(timestamp, voltage_v, current_a, connected, + selected_source, priority, throttle_normalized, armed, false); + + // Before refactoring and adding the BatteryBase and AnalogBattery classes, the only place that checked the + // value of the BAT_SOURCE parameter was in the ADC polling in sensors.cpp. So I keep that logic here for now. + if (_get_source() == 0) { + _get_battery_base().publish(); + } +} + +/** + * Whether the ADC channel for the voltage of this battery is valid. + * Corresponds to BOARD_BRICK_VALID_LIST + */ +bool AnalogBattery::is_valid() +{ +#ifdef BOARD_BRICK_VALID_LIST + bool valid[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST; + return valid[_get_brick_index()]; +#else + // TODO: Maybe return false instead? + return true; +#endif +} + +int AnalogBattery::getVoltageChannel() +{ + if (_get_adc_channel() >= 0) { + return _get_adc_channel(); + + } else { + return DEFAULT_V_CHANNEL[_get_brick_index()]; + } +} + +int AnalogBattery::getCurrentChannel() +{ + // TODO: Possibly implement parameter for current sense channel + return DEFAULT_I_CHANNEL[_get_brick_index()]; +} + +float +AnalogBattery::_get_cnt_v_volt() +{ + float val = _get_cnt_v_volt_raw(); + + + if (val < 0.0f) { + // TODO: This magic constant was hardcoded into sensors.cpp before I did the refactor. I don't know + // what the best way is to make it not a magic number. + return DEFAULT_VOLTS_PER_COUNT; + + } else { + return val; + } +} + +float +AnalogBattery::_get_cnt_v_curr() +{ + float val = _get_cnt_v_curr_raw(); + + if (val < 0.0f) { + return DEFAULT_VOLTS_PER_COUNT; + + } else { + return val; + } +} + +float +AnalogBattery::_get_v_div() +{ + float val = _get_v_div_raw(); + + if (val <= 0.0f) { + return BOARD_BATTERY1_V_DIV; + + } else { + return val; + } +} + +float +AnalogBattery::_get_a_per_v() +{ + float val = _get_a_per_v_raw(); + + if (val <= 0.0f) { + return BOARD_BATTERY1_A_PER_V; + + } else { + return val; + } +} + +#if BOARD_NUMBER_BRICKS > 0 +AnalogBattery1::AnalogBattery1() +{ + Battery1::migrateParam(_param_old_a_per_v, _param_a_per_v, "A_PER_V", -1.0f); + Battery1::migrateParam(_param_old_adc_channel, _param_adc_channel, "ADC_CHANNEL", -1); + Battery1::migrateParam(_param_old_v_div, _param_v_div, "V_DIV", -1.0f); +} +#endif \ No newline at end of file diff --git a/src/modules/sensors/analog_battery.h b/src/modules/sensors/analog_battery.h new file mode 100644 index 0000000000..be340329a7 --- /dev/null +++ b/src/modules/sensors/analog_battery.h @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +class AnalogBattery : public ModuleParams +{ +public: + AnalogBattery(); + + /** + * 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(hrt_abstime timestamp, int32_t voltage_raw, int32_t current_raw, + bool selected_source, int priority, float throttle_normalized, + bool armed); + + /** + * Whether the ADC channel for the voltage of this battery is valid. + * Corresponds to BOARD_BRICK_VALID_LIST + */ + bool is_valid(); + + /** + * Which ADC channel is used for voltage reading of this battery + */ + int getVoltageChannel(); + + /** + * Which ADC channel is used for current reading of this battery + */ + int getCurrentChannel(); + +protected: + 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_adc_channel() = 0; + virtual int _get_source() = 0; + + virtual int _get_brick_index() = 0; + + virtual BatteryBase &_get_battery_base() = 0; + + float _get_cnt_v_volt(); + float _get_cnt_v_curr(); + float _get_v_div(); + float _get_a_per_v(); +}; + +#if BOARD_NUMBER_BRICKS > 0 +class AnalogBattery1 : public AnalogBattery +{ +public: + AnalogBattery1(); +protected: + + Battery1 _base{}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_old_v_div, + (ParamFloat) _param_old_a_per_v, + (ParamInt) _param_old_adc_channel, + + (ParamFloat) _param_v_div, + (ParamFloat) _param_a_per_v, + (ParamInt) _param_adc_channel, + + (ParamFloat) _param_cnt_v_volt, + (ParamFloat) _param_cnt_v_curr, + (ParamFloat) _param_v_offs_cur, + (ParamInt) _param_source + ) + + 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_adc_channel() override {return _param_adc_channel.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(); } + int _get_source() override {return _param_source.get();} + + int _get_brick_index() override {return 0; } + + BatteryBase &_get_battery_base() override {return _base;} +}; +#endif // #if BOARD_NUMBER_BRICKS > 0 + +#if BOARD_NUMBER_BRICKS > 1 +class AnalogBattery2 : public AnalogBattery +{ +protected: + + Battery2 _base{}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_v_div, + (ParamFloat) _param_a_per_v, + (ParamInt) _param_adc_channel, + + (ParamFloat) _param_cnt_v_volt, + (ParamFloat) _param_cnt_v_curr, + (ParamFloat) _param_v_offs_cur, + (ParamInt) _param_source + ) + + + 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_adc_channel() override {return _param_adc_channel.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(); } + int _get_source() override {return _param_source.get();} + + int _get_brick_index() override {return 1; } + + BatteryBase &_get_battery_base() override {return _base;} +}; +#endif // #if BOARD_NUMBER_BRICKS > 1 \ No newline at end of file diff --git a/src/modules/sensors/power.cpp b/src/modules/sensors/power.cpp index c80f52497a..de1b963545 100644 --- a/src/modules/sensors/power.cpp +++ b/src/modules/sensors/power.cpp @@ -59,11 +59,11 @@ void Power::update(px4_adc_msg_t buf_adc[PX4_MAX_ADC_CHANNELS], int nchannels, f 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) { + if (_analogBatteries[b]->getVoltageChannel() == 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) { + } else if (_analogBatteries[b]->getCurrentChannel() == buf_adc[i].am_channel) { /* Voltage at current sense resistor in ADC counts */ bat_current_cnt[b] = buf_adc[i].am_data; } @@ -82,7 +82,7 @@ void Power::update(px4_adc_msg_t buf_adc[PX4_MAX_ADC_CHANNELS], int nchannels, f selected_source = b; } - _analogBatteries[b]->updateBatteryStatusRawADC(bat_voltage_cnt[b], bat_current_cnt[b], hrt_absolute_time(), + _analogBatteries[b]->updateBatteryStatusRawADC(hrt_absolute_time(), bat_voltage_cnt[b], bat_current_cnt[b], selected_source == b, b, throttle, armed); } diff --git a/src/modules/sensors/power.h b/src/modules/sensors/power.h index 1f47441e01..06e2f44b25 100644 --- a/src/modules/sensors/power.h +++ b/src/modules/sensors/power.h @@ -40,13 +40,7 @@ #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 +#include "analog_battery.h" /** * Measures voltage, current, etc. of all batteries connected to the vehicle, both @@ -80,8 +74,8 @@ private: * * For an example of what this looks like after preprocessing, assume that BOARD_NUMBER_BRICKS = 2: * ``` - * Battery1 _battery0; - * Battery2 _battery1; + * AnalogBattery1 _battery0; + * AnalogBattery2 _battery1; * * BatteryBase *_analogBatteries[2] { * &_battery0, @@ -92,17 +86,14 @@ private: * 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; + AnalogBattery1 _battery0; #if BOARD_NUMBER_BRICKS > 1 - Battery2 _battery1; + AnalogBattery2 _battery1; #endif - BatteryBase *_analogBatteries[BOARD_NUMBER_BRICKS] { + AnalogBattery *_analogBatteries[BOARD_NUMBER_BRICKS] { &_battery0, #if BOARD_NUMBER_BRICKS > 1 &_battery1, diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5d1ed7b249..f69bb17647 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -72,7 +72,7 @@ #include "parameters.h" #include "voted_sensors_update.h" - +#include "power.h" #include "vehicle_acceleration/VehicleAcceleration.hpp" #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index bc9db1a273..badedc92dd 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -42,7 +42,7 @@ #pragma once -#include +#include #include #include #include @@ -183,8 +183,6 @@ private: _gps.writeData(&gps_data); - _battery_status.timestamp = hrt_absolute_time(); - _px4_accel.set_sample_rate(250); _px4_gyro.set_sample_rate(250); } @@ -237,10 +235,24 @@ private: hrt_abstime _last_sim_timestamp{0}; hrt_abstime _last_sitl_timestamp{0}; + hrt_abstime _last_battery_timestamp{0}; - // Lib used to do the battery calculations. - Battery1 _battery {}; - battery_status_s _battery_status{}; + // Because the simulator doesn't actually care about the real values of these, just stick + // in some good defaults. + // This is an anonymous class, because BatteryBase is abstract and can't directly be instantiated. + class : public BatteryBase + { + float _get_bat_v_empty() override { return 3.5; } + float _get_bat_v_charged() override { return 4.05; } + int _get_bat_n_cells() override { return 4; } + float _get_bat_capacity() override { return 10.0; } + float _get_bat_v_load_drop() override { return 0; } + float _get_bat_r_internal() override { return 0; } + float _get_bat_low_thr() override { return 0.15; } + float _get_bat_crit_thr() override { return 0.07; } + float _get_bat_emergen_thr() override { return 0.05; } + int _get_source() override { return 0; } + } _battery {}; #ifndef __PX4_QURT diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 8b54244428..5b4274df9f 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -346,7 +346,7 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) static uint64_t last_integration_us = 0; // battery simulation (limit update to 100Hz) - if (hrt_elapsed_time(&_battery_status.timestamp) >= 10_ms) { + if (hrt_elapsed_time(&_last_battery_timestamp) >= 10_ms) { const float discharge_interval_us = _param_sim_bat_drain.get() * 1000 * 1000; @@ -370,7 +370,9 @@ 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(vbatt, ibatt, now_us, true, 0, throttle, armed); + _battery.updateBatteryStatus(now_us, vbatt, ibatt, true, true, 0, throttle, armed, true); + + _last_battery_timestamp = now_us; } }