From 993fa5bd3758030e6b461109f4b0ae8da9cec388 Mon Sep 17 00:00:00 2001 From: Timothy Scott Date: Thu, 7 Nov 2019 15:59:12 +0100 Subject: [PATCH] Refactored to work with new battery_status module --- .../crazyflie/syslink/syslink_main.cpp | 2 +- .../bitcraze/crazyflie/syslink/syslink_main.h | 2 +- boards/intel/aerofc-v1/default.cmake | 4 +- msg/battery_status.msg | 1 + .../df_bebop_bus_wrapper.cpp | 10 +- .../df_ltc2946_wrapper/df_ltc2946_wrapper.cpp | 2 +- src/drivers/power_monitor/voxlpm/voxlpm.cpp | 10 +- src/drivers/power_monitor/voxlpm/voxlpm.hpp | 2 - src/lib/battery/CMakeLists.txt | 7 +- src/lib/battery/battery.cpp | 351 +++++++++++++++++- src/lib/battery/battery.h | 337 ++++++++++------- src/lib/battery/battery_base.cpp | 256 ------------- src/lib/battery/battery_base.h | 183 --------- src/lib/battery/battery_params_1.c | 184 --------- src/lib/battery/battery_params_2.c | 180 --------- src/lib/battery/battery_params_common.c | 54 --- src/lib/battery/battery_params_deprecated.c | 38 +- src/lib/battery/module.yaml | 140 +++++++ src/modules/battery_status/CMakeLists.txt | 5 +- src/modules/battery_status/analog_battery.cpp | 151 ++++++++ src/modules/battery_status/analog_battery.h | 104 ++++++ ...ttery.c => analog_battery_params_common.c} | 25 -- ...s.h => analog_battery_params_deprecated.c} | 61 +-- src/modules/battery_status/battery_status.cpp | 121 ++---- src/modules/battery_status/module.yaml | 50 +++ src/modules/battery_status/parameters.cpp | 119 ------ src/modules/mavlink/mavlink_messages.cpp | 3 +- src/modules/sensors/CMakeLists.txt | 2 - src/modules/sensors/analog_battery.cpp | 141 ------- src/modules/sensors/analog_battery.h | 163 -------- src/modules/sensors/power.cpp | 91 ----- src/modules/sensors/power.h | 103 ----- src/modules/sensors/sensors.cpp | 1 - src/modules/simulator/simulator.h | 33 +- validation/module_schema.yaml | 2 +- 35 files changed, 1101 insertions(+), 1837 deletions(-) delete mode 100644 src/lib/battery/battery_base.cpp delete mode 100644 src/lib/battery/battery_base.h delete mode 100644 src/lib/battery/battery_params_1.c delete mode 100644 src/lib/battery/battery_params_2.c create mode 100644 src/lib/battery/module.yaml create mode 100644 src/modules/battery_status/analog_battery.cpp create mode 100644 src/modules/battery_status/analog_battery.h rename src/modules/battery_status/{sensor_params_battery.c => analog_battery_params_common.c} (79%) rename src/modules/battery_status/{parameters.h => analog_battery_params_deprecated.c} (58%) create mode 100644 src/modules/battery_status/module.yaml delete mode 100644 src/modules/battery_status/parameters.cpp delete mode 100644 src/modules/sensors/analog_battery.cpp delete mode 100644 src/modules/sensors/analog_battery.h delete mode 100644 src/modules/sensors/power.cpp delete mode 100644 src/modules/sensors/power.h diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index 682c9305c6..72606d4333 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp @@ -420,7 +420,7 @@ Syslink::handle_message(syslink_message_t *msg) } /* With the usb plugged in and battery disconnected, it appears to be charged. The voltage check ensures that a battery is connected */ - else if (powered && !charging && _battery_status.voltage_filtered_v > 3.7f) { + else if (powered && !charging && vbat > 3.7f) { _bstate = BAT_CHARGED; } else { diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.h b/boards/bitcraze/crazyflie/syslink/syslink_main.h index d8495f1750..e6e7f14079 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.h +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.h @@ -137,7 +137,7 @@ private: uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; - Battery1 _battery; + Battery _battery; int32_t _rssi; battery_state _bstate; diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index 832507cdf4..7e375f0b7e 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -32,7 +32,7 @@ px4_add_board( #uavcan MODULES #airspeed_selector - attitude_estimator_q + #attitude_estimator_q battery_status #camera_feedback commander @@ -44,7 +44,7 @@ px4_add_board( land_detector landing_target_estimator load_mon - local_position_estimator + #local_position_estimator logger mavlink mc_att_control diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 738732436a..a0d3d8be27 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -17,6 +17,7 @@ uint16 cycle_count # number of discharge cycles the battery has experien uint16 run_time_to_empty # predicted remaining battery capacity based on the present rate of discharge in min uint16 average_time_to_empty # predicted remaining battery capacity based on the average rate of discharge in min uint16 serial_number # serial number of the battery pack +uint8 id # ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed. float32[4] voltage_cell_v # Battery individual cell voltages float32 max_cell_voltage_delta # Max difference between individual cell voltages 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 7b3597999f..39f0a60ed1 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; - Battery1 _battery; + Battery _battery; bool _armed; float _last_throttle; @@ -203,6 +203,10 @@ int DfBebopBusWrapper::_publish(struct bebop_state_data &data) 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, false); + esc_status_s esc_status = {}; uint16_t esc_speed_setpoint_rpm[4] = {}; @@ -218,9 +222,7 @@ int DfBebopBusWrapper::_publish(struct bebop_state_data &data) // TODO: when is this ever blocked? if (!(m_pub_blocked)) { - // 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, true); + _battery.publish(); 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 15aa9674d0..47ac036064 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 @@ -69,7 +69,7 @@ public: private: int _publish(const struct ltc2946_sensor_data &data); - Battery1 _battery{}; + Battery _battery{}; int _actuator_ctrl_0_sub{-1}; int _vcontrol_mode_sub{-1}; diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.cpp b/src/drivers/power_monitor/voxlpm/voxlpm.cpp index 3c34164164..f47b790faa 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.cpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.cpp @@ -78,7 +78,7 @@ VOXLPM::init() write_reg(DEFAULT_CTRLA_REG_VAL, VOXLPM_LTC2946_CTRLA_REG); write_reg(DEFAULT_CTRLB_REG_VAL, VOXLPM_LTC2946_CTRLB_REG); - _battery.reset(&_bat_status); + _battery.reset(); start(); @@ -153,9 +153,7 @@ VOXLPM::measure() switch (_ch_type) { case VOXLPM_CH_TYPE_VBATT: { - _battery.updateBatteryStatus(tnow, _voltage, _amperage, true, true, 0, 0, false, &_bat_status); - - _bat_pub_topic.publish(_bat_status); + _battery.updateBatteryStatus(tnow, _voltage, _amperage, true, true, 0, 0, false, true); } // fallthrough @@ -176,9 +174,7 @@ VOXLPM::measure() } else { switch (_ch_type) { case VOXLPM_CH_TYPE_VBATT: { - _battery.updateBatteryStatus(tnow, 0.0, 0.0, true, true, 0, 0, false, &_bat_status); - - _bat_pub_topic.publish(_bat_status); + _battery.updateBatteryStatus(tnow, 0.0, 0.0, true, true, 0, 0, false, true); } break; diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.hpp b/src/drivers/power_monitor/voxlpm/voxlpm.hpp index 91dc6ffa80..90c1cbdf4c 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.hpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.hpp @@ -160,10 +160,8 @@ private: perf_counter_t _sample_perf; - uORB::PublicationMulti _bat_pub_topic{ORB_ID(battery_status)}; uORB::PublicationMulti _pm_pub_topic{ORB_ID(power_monitor)}; - battery_status_s _bat_status{}; power_monitor_s _pm_status{}; VOXLPM_CH_TYPE _ch_type; diff --git a/src/lib/battery/CMakeLists.txt b/src/lib/battery/CMakeLists.txt index 698be14941..684fe2e8ff 100644 --- a/src/lib/battery/CMakeLists.txt +++ b/src/lib/battery/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2018 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,4 +31,7 @@ # ############################################################################ -px4_add_library(battery battery_base.cpp battery.cpp) +px4_add_library(battery battery.cpp) + +# TODO: Add an option in px4_add_library function to add module config file +set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/module.yaml) diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 80569edd3a..c95a8e28b9 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -1,14 +1,349 @@ +/**************************************************************************** + * + * 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.cpp + * + * Library calls for battery functionality. + * + * @author Julian Oes + * @author Timothy Scott + */ #include "battery.h" +#include +#include +#include -Battery1::Battery1() +Battery::Battery(int index, ModuleParams *parent) : + ModuleParams(parent), + _index(index < 1 || index > 9 ? 1 : index), + _warning(battery_status_s::BATTERY_WARNING_NONE), + _last_timestamp(0) { - updateParams(); + if (index > 9 || index < 1) { + PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index); + } - 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); + // 16 chars for parameter name + null terminator + char param_name[17]; + + snprintf(param_name, sizeof(param_name), "BAT%d_V_EMPTY", _index); + _param_handles.v_empty = param_find(param_name); + + if (_param_handles.v_empty == PARAM_INVALID) { + PX4_ERR("Could not find parameter with name %s", param_name); + } + + snprintf(param_name, sizeof(param_name), "BAT%d_V_CHARGED", _index); + _param_handles.v_charged = param_find(param_name); + + snprintf(param_name, sizeof(param_name), "BAT%d_N_CELLS", _index); + _param_handles.n_cells = param_find(param_name); + + snprintf(param_name, sizeof(param_name), "BAT%d_CAPACITY", _index); + _param_handles.capacity = param_find(param_name); + + snprintf(param_name, sizeof(param_name), "BAT%d_V_LOAD_DROP", _index); + _param_handles.v_load_drop = param_find(param_name); + + snprintf(param_name, sizeof(param_name), "BAT%d_R_INTERNAL", _index); + _param_handles.r_internal = param_find(param_name); + + snprintf(param_name, sizeof(param_name), "BAT%d_SOURCE", _index); + _param_handles.source = param_find(param_name); + + _param_handles.low_thr = param_find("BAT_LOW_THR"); + _param_handles.crit_thr = param_find("BAT_CRIT_THR"); + _param_handles.emergen_thr = param_find("BAT_EMERGEN_THR"); + + _param_handles.v_empty_old = param_find("BAT_V_EMPTY"); + _param_handles.v_charged_old = param_find("BAT_V_CHARGED"); + _param_handles.n_cells_old = param_find("BAT_N_CELLS"); + _param_handles.capacity_old = param_find("BAT_CAPACITY"); + _param_handles.v_load_drop_old = param_find("BAT_V_LOAD_DROP"); + _param_handles.r_internal_old = param_find("BAT_R_INTERNAL"); + _param_handles.source_old = param_find("BAT_SOURCE"); + + updateParams(); +} + +void +Battery::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 = _params.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.capacity = _params.capacity; + _battery_status.temperature = NAN; + _battery_status.id = (uint8_t) _index; +} + +void +Battery::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) +{ + reset(); + _battery_status.timestamp = timestamp; + filterVoltage(voltage_v); + filterThrottle(throttle_normalized); + filterCurrent(current_a); + sumDischarged(timestamp, current_a); + estimateRemaining(_voltage_filtered_v, _current_filtered_a, _throttle_filtered, armed); + computeScale(); + + if (_battery_initialized) { + determineWarning(connected); + } + + 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.timestamp = timestamp; + + if (should_publish) { + publish(); + } +} + +void +Battery::publish() +{ + orb_publish_auto(ORB_ID(battery_status), &_orb_advert, &_battery_status, &_orb_instance, ORB_PRIO_DEFAULT); +} + +void +Battery::swapUorbAdvert(Battery &other) +{ + orb_advert_t tmp = _orb_advert; + _orb_advert = other._orb_advert; + other._orb_advert = tmp; +} + +void +Battery::filterVoltage(float voltage_v) +{ + if (!_battery_initialized) { + _voltage_filtered_v = voltage_v; + } + + // TODO: inspect that filter performance + const float filtered_next = _voltage_filtered_v * 0.99f + voltage_v * 0.01f; + + if (PX4_ISFINITE(filtered_next)) { + _voltage_filtered_v = filtered_next; + } +} + +void +Battery::filterCurrent(float current_a) +{ + if (!_battery_initialized) { + _current_filtered_a = current_a; + } + + // ADC poll is at 100Hz, this will perform a low pass over approx 500ms + const float filtered_next = _current_filtered_a * 0.98f + current_a * 0.02f; + + if (PX4_ISFINITE(filtered_next)) { + _current_filtered_a = filtered_next; + } +} + +void Battery::filterThrottle(float throttle) +{ + if (!_battery_initialized) { + _throttle_filtered = throttle; + } + + const float filtered_next = _throttle_filtered * 0.99f + throttle * 0.01f; + + if (PX4_ISFINITE(filtered_next)) { + _throttle_filtered = filtered_next; + } +} + +void +Battery::sumDischarged(hrt_abstime timestamp, float current_a) +{ + // Not a valid measurement + if (current_a < 0.f) { + // Because the measurement was invalid we need to stop integration + // and re-initialize with the next valid measurement + _last_timestamp = 0; + return; + } + + // Ignore first update because we don't know dt. + if (_last_timestamp != 0) { + const float dt = (timestamp - _last_timestamp) / 1e6; + // mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h]) + _discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f); + _discharged_mah += _discharged_mah_loop; + } + + _last_timestamp = timestamp; +} + +void +Battery::estimateRemaining(float voltage_v, float current_a, float throttle, bool armed) +{ + // remaining battery capacity based on voltage + float cell_voltage = voltage_v / _params.n_cells; + + // correct battery voltage locally for load drop to avoid estimation fluctuations + if (_params.r_internal >= 0.f) { + cell_voltage += _params.r_internal * current_a; + + } else { + // assume linear relation between throttle and voltage drop + cell_voltage += throttle * _params.v_load_drop; + } + + _remaining_voltage = math::gradual(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f); + + // choose which quantity we're using for final reporting + if (_params.capacity > 0.f) { + // if battery capacity is known, fuse voltage measurement with used capacity + if (!_battery_initialized) { + // initialization of the estimation state + _remaining = _remaining_voltage; + + } else { + // The lower the voltage the more adjust the estimate with it to avoid deep discharge + 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 / _params.capacity; + _remaining = math::max(_remaining, 0.f); + } + + } else { + // else use voltage + _remaining = _remaining_voltage; + } +} + +void +Battery::determineWarning(bool connected) +{ + if (connected) { + // propagate warning state only if the state is higher, otherwise remain in current warning state + if (_remaining < _params.emergen_thr) { + _warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + + } else if (_remaining < _params.crit_thr) { + _warning = battery_status_s::BATTERY_WARNING_CRITICAL; + + } else if (_remaining < _params.low_thr) { + _warning = battery_status_s::BATTERY_WARNING_LOW; + + } else { + _warning = battery_status_s::BATTERY_WARNING_NONE; + } + } +} + +void +Battery::computeScale() +{ + const float voltage_range = (_params.v_charged - _params.v_empty); + + // reusing capacity calculation to get single cell voltage before drop + const float bat_v = _params.v_empty + (voltage_range * _remaining_voltage); + + _scale = _params.v_charged / bat_v; + + if (_scale > 1.3f) { // Allow at most 30% compensation + _scale = 1.3f; + + } else if (!PX4_ISFINITE(_scale) || _scale < 1.f) { // Shouldn't ever be more than the power at full battery + _scale = 1.f; + } +} + +void Battery::updateParams() +{ + if (_index == 1) { + migrateParam(_param_handles.v_empty_old, _param_handles.v_empty, &_params.v_empty_old, &_params.v_empty, + _first_parameter_update); + migrateParam(_param_handles.v_charged_old, _param_handles.v_charged, &_params.v_charged_old, &_params.v_charged, + _first_parameter_update); + migrateParam(_param_handles.n_cells_old, _param_handles.n_cells, &_params.n_cells_old, &_params.n_cells, + _first_parameter_update); + migrateParam(_param_handles.capacity_old, _param_handles.capacity, &_params.capacity_old, &_params.capacity, + _first_parameter_update); + migrateParam(_param_handles.v_load_drop_old, _param_handles.v_load_drop, &_params.v_load_drop_old, + &_params.v_load_drop, _first_parameter_update); + migrateParam(_param_handles.r_internal_old, _param_handles.r_internal, &_params.r_internal_old, + &_params.r_internal, _first_parameter_update); + migrateParam(_param_handles.source_old, _param_handles.source, &_params.source_old, &_params.source, + _first_parameter_update); + + } else { + param_get(_param_handles.v_empty, &_params.v_empty); + param_get(_param_handles.v_charged, &_params.v_charged); + param_get(_param_handles.n_cells, &_params.n_cells); + param_get(_param_handles.capacity, &_params.capacity); + param_get(_param_handles.v_load_drop, &_params.v_load_drop); + param_get(_param_handles.r_internal, &_params.r_internal); + param_get(_param_handles.source, &_params.source); + } + + param_get(_param_handles.low_thr, &_params.low_thr); + param_get(_param_handles.crit_thr, &_params.crit_thr); + param_get(_param_handles.emergen_thr, &_params.emergen_thr); + + ModuleParams::updateParams(); + + _first_parameter_update = false; } diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 1ef5ee13d8..e61aaef5f0 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -31,157 +31,228 @@ * ****************************************************************************/ -#pragma once - -#include "battery_base.h" - -#include -#include - /** * @file battery.h - * 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 `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. + * Library calls for battery functionality. * - * 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); - * 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 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. + * @author Julian Oes + * @author Timothy Scott */ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + /** - * Battery1 represents a battery calibrated by BAT1_* parameters. + * BatteryBase is a base class for any type of battery. + * + * You can use this class on its own. Or, if you need to implement a custom battery type, + * you can inherit from this class. See, for example, src/modules/battery_status/AnalogBattery.h */ -class Battery1 : public BatteryBase +class Battery : public ModuleParams { public: - Battery1(); + Battery(int index = 1, ModuleParams *parent = nullptr); /** - * 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_*(...) + * Reset all battery stats and report invalid/nothing. */ - template static void - migrateParam(P1 &oldParam, P2 &newParam, const char *name, T defaultValue) - { - float diffOld = fabs((float) oldParam.get() - defaultValue); - float diffNew = fabs((float) newParam.get() - defaultValue); + void reset(); - 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(); + /** + * Get the battery cell count + */ + int cell_count() { return _params.n_cells; } + + /** + * Get the empty voltage per cell + */ + float empty_cell_voltage() { return _params.v_empty; } + + /** + * Get the full voltage per cell + */ + float full_cell_voltage() { return _params.v_charged; } + + int source() { return _params.source; } + + /** + * Update current battery status message. + * + * @param voltage_raw: Battery voltage, in Volts + * @param current_raw: Battery current, 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 + * @param should_publish If True, this function published a battery_status uORB message. + */ + 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); + + /** + * Publishes the uORB battery_status message with the most recently-updated data. + */ + void publish(); + + /** + * Some old functionality expects the primary battery to be published on instance 0. To maintain backwards + * compatibility, this function allows the advertisements (and therefore instances) of 2 batteries to be swapped. + * However, this should not be relied upon anywhere, and should be considered for all intents deprecated. + * + * The proper way to uniquely identify batteries is by the `id` field in the `battery_status` message. + */ + void swapUorbAdvert(Battery &other); + +protected: + struct { + param_t v_empty; + param_t v_charged; + param_t n_cells; + param_t capacity; + param_t v_load_drop; + param_t r_internal; + param_t low_thr; + param_t crit_thr; + param_t emergen_thr; + param_t source; + + // TODO: These parameters are depracated. They can be removed entirely once the + // new version of Firmware has been around for long enough. + param_t v_empty_old; + param_t v_charged_old; + param_t n_cells_old; + param_t capacity_old; + param_t v_load_drop_old; + param_t r_internal_old; + param_t source_old; + } _param_handles; + + struct { + float v_empty; + float v_charged; + int n_cells; + float capacity; + float v_load_drop; + float r_internal; + float low_thr; + float crit_thr; + float emergen_thr; + int source; + + // TODO: These parameters are depracated. They can be removed entirely once the + // new version of Firmware has been around for long enough. + float v_empty_old; + float v_charged_old; + int n_cells_old; + float capacity_old; + float v_load_drop_old; + float r_internal_old; + int source_old; + } _params; + + battery_status_s _battery_status; + + const int _index; + + bool _first_parameter_update{false}; + virtual void updateParams() override; + + /** + * This function helps with migrating to new parameters. It performs several tasks: + * - Update both the old and new parameter values using `param_get(...)` + * - Check if either parameter changed just now + * - If so, display a warning if the deprecated parameter was used + * - Copy the new value over to the other parameter + * - If this is the first time the parameters are fetched, check if they are equal + * - If not, display a warning and copy the value of the deprecated parameter over to the new one + * @tparam T Type of the parameter (int or float) + * @param old_param Handle to the old deprecated parameter (for example, param_find("BAT_N_CELLS") + * @param new_param Handle to the new replacement parameter (for example, param_find("BAT1_N_CELLS") + * @param old_val Pointer to the value of the old deprecated parameter + * @param new_val Pointer to the value of the new replacement parameter + * @param firstcall If true, then this function will not check to see if the values have changed + * (Since the old values are uninitialized) + * @return True iff either of these parameters changed just now and the migration was done. + */ + template + bool migrateParam(param_t old_param, param_t new_param, T *old_val, T *new_val, bool firstcall) + { + + T previous_old_val = *old_val; + T previous_new_val = *new_val; + + param_get(old_param, old_val); + param_get(new_param, new_val); + + if (!firstcall) { + if ((float) fabs((float) *old_val - (float) previous_old_val) > FLT_EPSILON + && (float) fabs((float) *old_val - (float) *new_val) > FLT_EPSILON) { + // TODO fix this error message. I was getting hardfaults while using this message, but they stopped + // after removing this message. I was unable to determine why I was getting them. + PX4_WARN("Detected change of deprecated parameter %s. Please use the new parameter %s instead. " + "The new value of the deprecated parameter will be copied to the new parameter.", + param_name(old_param), param_name(new_param)); + param_set_no_notification(new_param, old_val); + param_get(new_param, new_val); + return true; + + } else if ((float) fabs((float) *new_val - (float) previous_new_val) > FLT_EPSILON + && (float) fabs((float) *old_val - (float) *new_val) > FLT_EPSILON) { + PX4_INFO("Copying new value for %s to deprecated parameter %s.", + param_name(new_param), param_name(old_param)); + param_set_no_notification(old_param, new_val); + param_get(old_param, old_val); + return true; + } + + } else { + if ((float) fabs((float) *old_val - (float) *new_val) > FLT_EPSILON) { + PX4_WARN("At boot, deprecated parameter %s is different from its replacement %s." + " The new value will be overwritten by the old one. This should happen only once.", + param_name(old_param), param_name(new_param)); + param_set_no_notification(new_param, old_val); + param_get(new_param, new_val); + return true; + } } + + return false; } 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_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, + 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; - (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, - (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(); } - int _get_source() override {return _param_source.get(); } + orb_advert_t _orb_advert{nullptr}; + int _orb_instance; }; - -/** - * Battery2 represents a battery calibrated by BAT2_* parameters. - */ -class Battery2 : public BatteryBase -{ -public: - Battery2() {} -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_bat_low_thr, - (ParamFloat) _param_bat_crit_thr, - (ParamFloat) _param_bat_emergen_thr, - (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(); } - int _get_source() override {return _param_source.get(); } -}; \ No newline at end of file diff --git a/src/lib/battery/battery_base.cpp b/src/lib/battery/battery_base.cpp deleted file mode 100644 index 7a6c382449..0000000000 --- a/src/lib/battery/battery_base.cpp +++ /dev/null @@ -1,256 +0,0 @@ -/**************************************************************************** - * - * 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.cpp - * - * Library calls for battery functionality. - * - * @author Julian Oes - * @author Timothy Scott - */ - -#include "battery_base.h" -#include -#include -#include - -BatteryBase::BatteryBase() : - ModuleParams(nullptr), - _warning(battery_status_s::BATTERY_WARNING_NONE), - _last_timestamp(0) -{ -} - -void -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 = _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.capacity = _get_bat_capacity(); -} - -void -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) -{ - reset(); - _battery_status.timestamp = timestamp; - filterVoltage(voltage_v); - filterThrottle(throttle_normalized); - filterCurrent(current_a); - sumDischarged(timestamp, current_a); - estimateRemaining(_voltage_filtered_v, _current_filtered_a, _throttle_filtered, armed); - computeScale(); - - if (_battery_initialized) { - determineWarning(connected); - } - - 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.timestamp = timestamp; - - if (should_publish) { - publish(); - } - - _battery_status.temperature = NAN; -} - -void -BatteryBase::publish() -{ - orb_publish_auto(ORB_ID(battery_status), &_orbAdvert, &_battery_status, &_orbInstance, ORB_PRIO_DEFAULT); -} - -void -BatteryBase::filterVoltage(float voltage_v) -{ - if (!_battery_initialized) { - _voltage_filtered_v = voltage_v; - } - - // TODO: inspect that filter performance - const float filtered_next = _voltage_filtered_v * 0.99f + voltage_v * 0.01f; - - if (PX4_ISFINITE(filtered_next)) { - _voltage_filtered_v = filtered_next; - } -} - -void -BatteryBase::filterCurrent(float current_a) -{ - if (!_battery_initialized) { - _current_filtered_a = current_a; - } - - // ADC poll is at 100Hz, this will perform a low pass over approx 500ms - const float filtered_next = _current_filtered_a * 0.98f + current_a * 0.02f; - - if (PX4_ISFINITE(filtered_next)) { - _current_filtered_a = filtered_next; - } -} - -void BatteryBase::filterThrottle(float throttle) -{ - if (!_battery_initialized) { - _throttle_filtered = throttle; - } - - const float filtered_next = _throttle_filtered * 0.99f + throttle * 0.01f; - - if (PX4_ISFINITE(filtered_next)) { - _throttle_filtered = filtered_next; - } -} - -void -BatteryBase::sumDischarged(hrt_abstime timestamp, float current_a) -{ - // Not a valid measurement - if (current_a < 0.f) { - // Because the measurement was invalid we need to stop integration - // and re-initialize with the next valid measurement - _last_timestamp = 0; - return; - } - - // Ignore first update because we don't know dt. - if (_last_timestamp != 0) { - const float dt = (timestamp - _last_timestamp) / 1e6; - // mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h]) - _discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f); - _discharged_mah += _discharged_mah_loop; - } - - _last_timestamp = timestamp; -} - -void -BatteryBase::estimateRemaining(float voltage_v, float current_a, float throttle, bool armed) -{ - // remaining battery capacity based on voltage - float cell_voltage = voltage_v / _get_bat_n_cells(); - - // correct battery voltage locally for load drop to avoid estimation fluctuations - 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 * _get_bat_v_load_drop(); - } - - _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 (_get_bat_capacity() > 0.f) { - // if battery capacity is known, fuse voltage measurement with used capacity - if (!_battery_initialized) { - // initialization of the estimation state - _remaining = _remaining_voltage; - - } else { - // The lower the voltage the more adjust the estimate with it to avoid deep discharge - 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 / _get_bat_capacity(); - _remaining = math::max(_remaining, 0.f); - } - - } else { - // else use voltage - _remaining = _remaining_voltage; - } -} - -void -BatteryBase::determineWarning(bool connected) -{ - if (connected) { - // propagate warning state only if the state is higher, otherwise remain in current warning state - if (_remaining < _get_bat_emergen_thr()) { - _warning = battery_status_s::BATTERY_WARNING_EMERGENCY; - - } else if (_remaining < _get_bat_crit_thr()) { - _warning = battery_status_s::BATTERY_WARNING_CRITICAL; - - } else if (_remaining < _get_bat_low_thr()) { - _warning = battery_status_s::BATTERY_WARNING_LOW; - - } else { - _warning = battery_status_s::BATTERY_WARNING_NONE; - } - } -} - -void -BatteryBase::computeScale() -{ - 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 = _get_bat_v_empty() + (voltage_range * _remaining_voltage); - - _scale = _get_bat_v_charged() / bat_v; - - if (_scale > 1.3f) { // Allow at most 30% compensation - _scale = 1.3f; - - } else if (!PX4_ISFINITE(_scale) || _scale < 1.f) { // Shouldn't ever be more than the power at full battery - _scale = 1.f; - } -} diff --git a/src/lib/battery/battery_base.h b/src/lib/battery/battery_base.h deleted file mode 100644 index a636efda9d..0000000000 --- a/src/lib/battery/battery_base.h +++ /dev/null @@ -1,183 +0,0 @@ -/**************************************************************************** - * - * 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. - * - * 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 -{ -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 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 - * @param should_publish If True, this function published a battery_status uORB message. - */ - 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); - - /** - * Publishes the uORB battery_status message with the most recently-updated data. - */ - void publish(); - - // TODO: Check that there was actually a parameter update - void updateParams() {ModuleParams::updateParams();} - -protected: - /** - * @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; - /** - * @return Source of battery data. 0 = internal power module. 1 = external, via Mavlink - */ - virtual int _get_source() = 0; - - battery_status_s _battery_status; - -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; -}; diff --git a/src/lib/battery/battery_params_1.c b/src/lib/battery/battery_params_1.c deleted file mode 100644 index fed4575afa..0000000000 --- a/src/lib/battery/battery_params_1.c +++ /dev/null @@ -1,184 +0,0 @@ -/**************************************************************************** - * - * 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 - * 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. - */ - -#include -#include - -/** - * Empty cell voltage (5C load) - * - * 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(BAT1_V_EMPTY, 3.5f); - -/** - * Full cell voltage (5C load) - * - * 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(BAT1_V_CHARGED, 4.05f); - -/** - * Voltage drop per cell on full throttle - * - * 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 BAT1_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(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 - * BAT1_V_LOAD_DROP for all calculations. - * - * @group Battery Calibration - * @unit Ohms - * @min -1.0 - * @max 0.2 - * @reboot_required true - */ -PARAM_DEFINE_FLOAT(BAT1_R_INTERNAL, -1.0f); - - -/** - * Number of cells for battery 1. - * - * 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(BAT1_N_CELLS, 0); - -/** - * Battery 1 capacity. - * - * 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(BAT1_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(BAT1_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(BAT1_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(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 deleted file mode 100644 index 5178d6c565..0000000000 --- a/src/lib/battery/battery_params_2.c +++ /dev/null @@ -1,180 +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. - * - ****************************************************************************/ - -/** - * @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 index 1def2d99d4..d0ffe0daea 100644 --- a/src/lib/battery/battery_params_common.c +++ b/src/lib/battery/battery_params_common.c @@ -39,10 +39,6 @@ * @author Julian Oes */ -#include -#include - - /** * Low threshold * @@ -92,53 +88,3 @@ PARAM_DEFINE_FLOAT(BAT_CRIT_THR, 0.07f); * @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/lib/battery/battery_params_deprecated.c b/src/lib/battery/battery_params_deprecated.c index 1ada9ef2f1..0544227857 100644 --- a/src/lib/battery/battery_params_deprecated.c +++ b/src/lib/battery/battery_params_deprecated.c @@ -146,36 +146,18 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 0); PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); /** - * This parameter is deprecated. Please use BAT1_V_DIV instead. + * This parameter is deprecated. Please use BAT1_SOURCE 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. + * Battery monitoring source. * - * @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. + * 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_ADC_CHANNEL, -1); \ No newline at end of file +PARAM_DEFINE_INT32(BAT_SOURCE, 0); \ No newline at end of file diff --git a/src/lib/battery/module.yaml b/src/lib/battery/module.yaml new file mode 100644 index 0000000000..0a7db5ccc6 --- /dev/null +++ b/src/lib/battery/module.yaml @@ -0,0 +1,140 @@ +__max_num_config_instances: &max_num_config_instances 2 + +module_name: battery + +parameters: + - group: Battery Calibration + definitions: + BAT${i}_V_EMPTY: + description: + short: Empty cell voltage (5C load) + long: | + 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. + + type: float + unit: V + decimal: 2 + increment: 0.01 + reboot_required: true + num_instances: *max_num_config_instances + instance_start: 1 + default: [3.5, 3.5] + + BAT${i}_V_CHARGED: + description: + short: Full cell voltage (5C load) + long: | + 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 + + type: float + unit: V + decimal: 2 + increment: 0.01 + reboot_required: true + num_instances: *max_num_config_instances + instance_start: 1 + default: [4.05, 4.05] + + BAT${i}_V_LOAD_DROP: + description: + short: Voltage drop per cell on full throttle + long: | + 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${i}_R_INTERNAL is + set. + + type: float + unit: V + min: 0.07 + max: 0.5 + decimal: 2 + increment: 0.01 + reboot_required: true + num_instances: *max_num_config_instances + instance_start: 1 + default: [0.3, 0.3] + + BAT${i}_R_INTERNAL: + description: + short: Explicitly defines the per cell internal resistance for battery ${i} + long: | + If non-negative, then this will be used in place of + BAT${i}_V_LOAD_DROP for all calculations. + + type: float + unit: Ohm + min: -1.0 + max: 0.2 + decimal: 2 + increment: 0.01 + reboot_required: true + num_instances: *max_num_config_instances + instance_start: 1 + default: [-1.0, -1.0] + + BAT${i}_N_CELLS: + description: + short: Number of cells for battery ${i}. + long: | + Defines the number of cells the attached battery consists of. + + type: enum + values: + 2: 2S Battery + 3: 3S Battery + 4: 4S Battery + 5: 5S Battery + 6: 6S Battery + 7: 7S Battery + 8: 8S Battery + 9: 9S Battery + 10: 10S Battery + 11: 11S Battery + 12: 12S Battery + 13: 13S Battery + 14: 14S Battery + 15: 15S Battery + 16: 16S Battery + reboot_required: true + num_instances: *max_num_config_instances + instance_start: 1 + default: [0, 0] + + BAT${i}_CAPACITY: + description: + short: Battery ${i} capacity. + long: | + Defines the capacity of battery ${i} in mAh. + type: float + unit: mAh + min: -1.0 + max: 100000 + decimal: 0 + increment: 50 + reboot_required: true + num_instances: *max_num_config_instances + instance_start: 1 + default: [-1.0, -1.0] + + BAT${i}_SOURCE: + description: + short: Battery ${i} monitoring source. + long: | + 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. + type: enum + unit: mAh + values: + 0: Power Module + 1: External + reboot_required: true + num_instances: *max_num_config_instances + instance_start: 1 + default: [0, 0] \ No newline at end of file diff --git a/src/modules/battery_status/CMakeLists.txt b/src/modules/battery_status/CMakeLists.txt index c01021aef6..29e5484ef9 100644 --- a/src/modules/battery_status/CMakeLists.txt +++ b/src/modules/battery_status/CMakeLists.txt @@ -36,7 +36,10 @@ px4_add_module( MAIN battery_status SRCS battery_status.cpp - parameters.cpp + analog_battery.cpp + + MODULE_CONFIG + module.yaml DEPENDS battery diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp new file mode 100644 index 0000000000..84cd018e53 --- /dev/null +++ b/src/modules/battery_status/analog_battery.cpp @@ -0,0 +1,151 @@ +#include +#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(int index, ModuleParams *parent) : + Battery(index, parent) +{ + char param_name[17]; + + _analog_param_handles.cnt_v_volt = param_find("BAT_CNT_V_VOLT"); + + _analog_param_handles.cnt_v_curr = param_find("BAT_CNT_V_CURR"); + + _analog_param_handles.v_offs_cur = param_find("BAT_V_OFFS_CURR"); + + snprintf(param_name, sizeof(param_name), "BAT%d_V_DIV", index); + _analog_param_handles.v_div = param_find(param_name); + + snprintf(param_name, sizeof(param_name), "BAT%d_A_PER_V", index); + _analog_param_handles.a_per_v = param_find(param_name); + + snprintf(param_name, sizeof(param_name), "BAT%d_ADC_CHANNEL", index); + _analog_param_handles.adc_channel = param_find(param_name); + + _analog_param_handles.v_div_old = param_find("BAT_V_DIV"); + _analog_param_handles.a_per_v_old = param_find("BAT_A_PER_V"); + _analog_param_handles.adc_channel_old = param_find("BAT_ADC_CHANNEL"); +} + +void +AnalogBattery::updateBatteryStatusRawADC(hrt_abstime timestamp, int32_t voltage_raw, int32_t current_raw, + bool selected_source, int priority, float throttle_normalized, + bool armed) +{ + float voltage_v = (voltage_raw * _analog_params.cnt_v_volt) * _analog_params.v_div; + float current_a = ((current_raw * _analog_params.cnt_v_curr) - _analog_params.v_offs_cur) * _analog_params.a_per_v; + + bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V && + (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid()); + + + Battery::updateBatteryStatus(timestamp, voltage_v, current_a, connected, + selected_source, priority, throttle_normalized, armed, _params.source == 0); +} + +/** + * 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[_index - 1]; +#else + // TODO: Maybe return false instead? + return true; +#endif +} + +int AnalogBattery::get_voltage_channel() +{ + if (_analog_params.adc_channel >= 0) { + return _analog_params.adc_channel; + + } else { + return DEFAULT_V_CHANNEL[_index - 1]; + } +} + +int AnalogBattery::get_current_channel() +{ + // TODO: Possibly implement parameter for current sense channel + return DEFAULT_I_CHANNEL[_index - 1]; +} + +void +AnalogBattery::updateParams() +{ + if (_index == 1) { + migrateParam(_analog_param_handles.v_div_old, _analog_param_handles.v_div, &_analog_params.v_div_old, + &_analog_params.v_div, _first_parameter_update); + migrateParam(_analog_param_handles.a_per_v_old, _analog_param_handles.a_per_v, &_analog_params.a_per_v_old, + &_analog_params.a_per_v, _first_parameter_update); + migrateParam(_analog_param_handles.adc_channel_old, _analog_param_handles.adc_channel, + &_analog_params.adc_channel_old, &_analog_params.adc_channel, _first_parameter_update); + + } else { + param_get(_analog_param_handles.v_div, &_analog_params.v_div); + param_get(_analog_param_handles.a_per_v, &_analog_params.a_per_v); + param_get(_analog_param_handles.adc_channel, &_analog_params.adc_channel); + } + + param_get(_analog_param_handles.cnt_v_volt, &_analog_params.cnt_v_volt); + param_get(_analog_param_handles.cnt_v_curr, &_analog_params.cnt_v_curr); + param_get(_analog_param_handles.v_offs_cur, &_analog_params.v_offs_cur); + + /* scaling of ADC ticks to battery voltage */ + if (_analog_params.cnt_v_volt < 0.0f) { + /* apply scaling according to defaults if set to default */ + _analog_params.cnt_v_volt = (BOARD_ADC_POS_REF_V_FOR_VOLTAGE_CHAN / px4_arch_adc_dn_fullcount()); + param_set_no_notification(_analog_param_handles.cnt_v_volt, &_analog_params.cnt_v_volt); + } + + /* scaling of ADC ticks to battery current */ + if (_analog_params.cnt_v_curr < 0.0f) { + /* apply scaling according to defaults if set to default */ + _analog_params.cnt_v_curr = (BOARD_ADC_POS_REF_V_FOR_CURRENT_CHAN / px4_arch_adc_dn_fullcount()); + param_set_no_notification(_analog_param_handles.cnt_v_curr, &_analog_params.cnt_v_curr); + } + + if (_analog_params.v_div <= 0.0f) { + /* apply scaling according to defaults if set to default */ + _analog_params.v_div = BOARD_BATTERY1_V_DIV; + param_set_no_notification(_analog_param_handles.v_div, &_analog_params.v_div); + + if (_index == 1) { + _analog_params.v_div_old = BOARD_BATTERY1_V_DIV; + param_set_no_notification(_analog_param_handles.v_div_old, &_analog_params.v_div_old); + } + } + + if (_analog_params.a_per_v <= 0.0f) { + /* apply scaling according to defaults if set to default */ + + _analog_params.a_per_v = BOARD_BATTERY1_A_PER_V; + param_set_no_notification(_analog_param_handles.a_per_v, &_analog_params.a_per_v); + + if (_index == 1) { + _analog_params.a_per_v_old = BOARD_BATTERY1_A_PER_V; + param_set_no_notification(_analog_param_handles.a_per_v_old, &_analog_params.a_per_v_old); + } + } + + Battery::updateParams(); +} \ No newline at end of file diff --git a/src/modules/battery_status/analog_battery.h b/src/modules/battery_status/analog_battery.h new file mode 100644 index 0000000000..72e89825b3 --- /dev/null +++ b/src/modules/battery_status/analog_battery.h @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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 Battery +{ +public: + AnalogBattery(int index = 1, ModuleParams *parent = nullptr); + + /** + * 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 get_voltage_channel(); + + /** + * Which ADC channel is used for current reading of this battery + */ + int get_current_channel(); + +protected: + + struct { + param_t cnt_v_volt; + param_t cnt_v_curr; + param_t v_offs_cur; + param_t v_div; + param_t a_per_v; + param_t adc_channel; + + param_t v_div_old; + param_t a_per_v_old; + param_t adc_channel_old; + } _analog_param_handles; + + struct { + float cnt_v_volt; + float cnt_v_curr; + float v_offs_cur; + float v_div; + float a_per_v; + int adc_channel; + + float v_div_old; + float a_per_v_old; + int adc_channel_old; + } _analog_params; + + virtual void updateParams() override; +}; diff --git a/src/modules/battery_status/sensor_params_battery.c b/src/modules/battery_status/analog_battery_params_common.c similarity index 79% rename from src/modules/battery_status/sensor_params_battery.c rename to src/modules/battery_status/analog_battery_params_common.c index 74bde8971a..7fbea4cb05 100644 --- a/src/modules/battery_status/sensor_params_battery.c +++ b/src/modules/battery_status/analog_battery_params_common.c @@ -65,28 +65,3 @@ PARAM_DEFINE_FLOAT(BAT_CNT_V_CURR, -1.0); * @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); - -/** - * Battery 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); diff --git a/src/modules/battery_status/parameters.h b/src/modules/battery_status/analog_battery_params_deprecated.c similarity index 58% rename from src/modules/battery_status/parameters.h rename to src/modules/battery_status/analog_battery_params_deprecated.c index 213d258254..d37d454803 100644 --- a/src/modules/battery_status/parameters.h +++ b/src/modules/battery_status/analog_battery_params_deprecated.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 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 @@ -31,54 +31,25 @@ * ****************************************************************************/ -#pragma once - /** - * @file parameters.h + * This parameter is deprecated. Please use BAT1_V_DIV. * - * defines the list of parameters that are used within the sensors module + * @group Battery Calibration + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0); + +/** + * This parameter is deprecated. Please use BAT1_A_PER_V. * - * @author Beat Kueng + * @group Battery Calibration + * @decimal 8 */ -#include -#include - -#include -#include - -namespace battery_status -{ - -struct Parameters { - float battery_voltage_scaling; - float battery_current_scaling; - float battery_current_offset; - float battery_v_div; - float battery_a_per_v; - int32_t battery_source; - int32_t battery_adc_channel; -}; - -struct ParameterHandles { - param_t battery_voltage_scaling; - param_t battery_current_scaling; - param_t battery_current_offset; - param_t battery_v_div; - param_t battery_a_per_v; - param_t battery_source; - param_t battery_adc_channel; -}; +PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0); /** - * initialize ParameterHandles struct + * This parameter is deprecated. Please use BAT1_ADC_CHANNEL. + * + * @group Battery Calibration */ -void initialize_parameter_handles(ParameterHandles ¶meter_handles); - - -/** - * Read out the parameters using the handles into the parameters struct. - * @return 0 on success, <0 on error - */ -int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters); - -} /* namespace sensors */ +PARAM_DEFINE_INT32(BAT_ADC_CHANNEL, -1); \ No newline at end of file diff --git a/src/modules/battery_status/battery_status.cpp b/src/modules/battery_status/battery_status.cpp index f6bab0100d..a60c2ba565 100644 --- a/src/modules/battery_status/battery_status.cpp +++ b/src/modules/battery_status/battery_status.cpp @@ -66,10 +66,9 @@ #include -#include "parameters.h" +#include "analog_battery.h" using namespace DriverFramework; -using namespace battery_status; using namespace time_literals; /** @@ -137,8 +136,18 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ - orb_advert_t _battery_pub[BOARD_NUMBER_BRICKS] {}; /**< battery status */ - Battery _battery[BOARD_NUMBER_BRICKS]; /**< Helper lib to publish battery_status topic. */ + AnalogBattery _battery1; + +#if BOARD_NUMBER_BRICKS > 1 + AnalogBattery _battery2; +#endif + + AnalogBattery *_analogBatteries[BOARD_NUMBER_BRICKS] { + &_battery1, +#if BOARD_NUMBER_BRICKS > 1 + &_battery2, +#endif + }; // End _analogBatteries #if BOARD_NUMBER_BRICKS > 1 int _battery_pub_intance0ndx {0}; /**< track the index of instance 0 */ @@ -146,16 +155,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - hrt_abstime _last_config_update{0}; - - Parameters _parameters{}; /**< local copies of interesting parameters */ - ParameterHandles _parameter_handles{}; /**< handles for interesting parameters */ - - /** - * Update our local parameter cache. - */ - int parameters_update(); - /** * Do adc-related initialisation. */ @@ -178,13 +177,13 @@ private: BatteryStatus::BatteryStatus() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + _battery1(1, this), +#if BOARD_NUMBER_BRICKS > 1 + _battery2(2, this), +#endif _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME)) { - initialize_parameter_handles(_parameter_handles); - - for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) { - _battery[b].setParent(this); - } + updateParams(); } BatteryStatus::~BatteryStatus() @@ -192,19 +191,6 @@ BatteryStatus::~BatteryStatus() ScheduleClear(); } -int -BatteryStatus::parameters_update() -{ - /* read the parameter values into _parameters */ - int ret = update_parameters(_parameter_handles, _parameters); - - if (ret) { - return ret; - } - - return ret; -} - int BatteryStatus::adc_init() { @@ -228,7 +214,6 @@ BatteryStatus::parameter_update_poll(bool forced) _parameter_update_sub.copy(&pupdate); // update parameters from storage - parameters_update(); updateParams(); } } @@ -242,30 +227,15 @@ BatteryStatus::adc_poll() /* read all channels available */ int ret = _h_adc.read(&buf_adc, sizeof(buf_adc)); - //todo:abosorb into new class Power - /* 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 */ - /* The ADC channels that are associated with each brick, in power controller - * priority order highest to lowest, as defined by the board config. - */ - int bat_voltage_v_chan[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST; - int bat_voltage_i_chan[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST; - - if (_parameters.battery_adc_channel >= 0) { // overwrite default - bat_voltage_v_chan[0] = _parameters.battery_adc_channel; - } - - /* The valid signals (HW dependent) are associated with each brick */ - bool valid_chan[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST; - /* Per Brick readings with default unread channels at 0 */ - float bat_current_a[BOARD_NUMBER_BRICKS] = {0.0f}; - float bat_voltage_v[BOARD_NUMBER_BRICKS] = {0.0f}; + int32_t bat_current_adc_readings[BOARD_NUMBER_BRICKS] {}; + int32_t bat_voltage_adc_readings[BOARD_NUMBER_BRICKS] {}; /* 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 @@ -284,7 +254,7 @@ BatteryStatus::adc_poll() /* Once we have subscriptions, Do this once for the lowest (highest priority * supply on power controller) that is valid. */ - if (_battery_pub[b] != nullptr && selected_source < 0 && valid_chan[b]) { + if (selected_source < 0 && _analogBatteries[b]->is_valid()) { /* Indicate the lowest brick (highest priority supply on power controller) * that is valid as the one that is the selected source for the * VDD_5V_IN @@ -294,54 +264,41 @@ BatteryStatus::adc_poll() /* Move the selected_source to instance 0 */ if (_battery_pub_intance0ndx != selected_source) { - - orb_advert_t tmp_h = _battery_pub[_battery_pub_intance0ndx]; - _battery_pub[_battery_pub_intance0ndx] = _battery_pub[selected_source]; - _battery_pub[selected_source] = tmp_h; + _analogBatteries[_battery_pub_intance0ndx]->swapUorbAdvert( + *_analogBatteries[selected_source] + ); _battery_pub_intance0ndx = selected_source; } # endif /* BOARD_NUMBER_BRICKS > 1 */ } - // todo:per brick scaling /* look for specific channels and process the raw voltage to measurement data */ - if (bat_voltage_v_chan[b] == buf_adc[i].am_channel) { + if (_analogBatteries[b]->get_voltage_channel() == buf_adc[i].am_channel) { /* Voltage in volts */ - bat_voltage_v[b] = (buf_adc[i].am_data * _parameters.battery_voltage_scaling) * _parameters.battery_v_div; + bat_voltage_adc_readings[b] = buf_adc[i].am_data; - } else if (bat_voltage_i_chan[b] == buf_adc[i].am_channel) { - bat_current_a[b] = ((buf_adc[i].am_data * _parameters.battery_current_scaling) - - _parameters.battery_current_offset) * _parameters.battery_a_per_v; + } else if (_analogBatteries[b]->get_current_channel() == buf_adc[i].am_channel) { + bat_current_adc_readings[b] = buf_adc[i].am_data; } } } } - if (_parameters.battery_source == 0) { - for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) { - - /* Consider the brick connected if there is a voltage */ - bool connected = bat_voltage_v[b] > BOARD_ADC_OPEN_CIRCUIT_V; - - /* In the case where the BOARD_ADC_OPEN_CIRCUIT_V is - * greater than the BOARD_VALID_UV let the HW qualify that it - * is connected. - */ - if (BOARD_ADC_OPEN_CIRCUIT_V > BOARD_VALID_UV) { - connected &= valid_chan[b]; - } - + for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) { + if (_analogBatteries[b]->source() == 0) { actuator_controls_s ctrl{}; _actuator_ctrl_0_sub.copy(&ctrl); - battery_status_s battery_status{}; - _battery[b].updateBatteryStatus(hrt_absolute_time(), bat_voltage_v[b], bat_current_a[b], - connected, selected_source == b, b, - ctrl.control[actuator_controls_s::INDEX_THROTTLE], - _armed, &battery_status); - int instance; - orb_publish_auto(ORB_ID(battery_status), &_battery_pub[b], &battery_status, &instance, ORB_PRIO_DEFAULT); + _analogBatteries[b]->updateBatteryStatusRawADC( + hrt_absolute_time(), + bat_voltage_adc_readings[b], + bat_current_adc_readings[b], + selected_source == b, + b, + ctrl.control[actuator_controls_s::INDEX_THROTTLE], + _armed + ); } } } diff --git a/src/modules/battery_status/module.yaml b/src/modules/battery_status/module.yaml new file mode 100644 index 0000000000..bf79791028 --- /dev/null +++ b/src/modules/battery_status/module.yaml @@ -0,0 +1,50 @@ +__max_num_config_instances: &max_num_config_instances 2 + +module_name: battery_status + +parameters: + - group: Battery Calibration + definitions: + BAT${i}_V_DIV: + description: + short: Battery ${i} voltage divider (V divider) + long: | + This is the divider from battery ${i} 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. + + type: float + decimal: 8 + reboot_required: true + num_instances: *max_num_config_instances + instance_start: 1 + default: [-1.0, -1.0] + + BAT${i}_A_PER_V: + description: + short: Battery ${i} current per volt (A/V) + long: | + 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. + + type: float + decimal: 8 + reboot_required: true + num_instances: *max_num_config_instances + instance_start: 1 + default: [-1.0, -1.0] + + BAT${i}_ADC_CHANNEL: + description: + short: Battery ${i} ADC Channel + long: | + This parameter specifies the ADC channel used to monitor voltage of main power battery. + A value of -1 means to use the board default. + + type: int32 + reboot_required: true + num_instances: *max_num_config_instances + instance_start: 1 + default: [-1, -1] diff --git a/src/modules/battery_status/parameters.cpp b/src/modules/battery_status/parameters.cpp deleted file mode 100644 index 0cca570069..0000000000 --- a/src/modules/battery_status/parameters.cpp +++ /dev/null @@ -1,119 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016-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 parameters.cpp - * - * @author Beat Kueng - */ - -#include "parameters.h" - -#include -#include -#include - -namespace battery_status -{ - -void initialize_parameter_handles(ParameterHandles ¶meter_handles) -{ - parameter_handles.battery_voltage_scaling = param_find("BAT_CNT_V_VOLT"); - parameter_handles.battery_current_scaling = param_find("BAT_CNT_V_CURR"); - parameter_handles.battery_current_offset = param_find("BAT_V_OFFS_CURR"); - parameter_handles.battery_v_div = param_find("BAT_V_DIV"); - parameter_handles.battery_a_per_v = param_find("BAT_A_PER_V"); - parameter_handles.battery_source = param_find("BAT_SOURCE"); - parameter_handles.battery_adc_channel = param_find("BAT_ADC_CHANNEL"); -} - -int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters) -{ - int ret = PX4_OK; - - const char *paramerr = "FAIL PARM LOAD"; - - /* scaling of ADC ticks to battery voltage */ - if (param_get(parameter_handles.battery_voltage_scaling, &(parameters.battery_voltage_scaling)) != OK) { - PX4_WARN("%s", paramerr); - - } else if (parameters.battery_voltage_scaling < 0.0f) { - /* apply scaling according to defaults if set to default */ - parameters.battery_voltage_scaling = (BOARD_ADC_POS_REF_V_FOR_VOLTAGE_CHAN / px4_arch_adc_dn_fullcount()); - param_set_no_notification(parameter_handles.battery_voltage_scaling, ¶meters.battery_voltage_scaling); - } - - /* scaling of ADC ticks to battery current */ - if (param_get(parameter_handles.battery_current_scaling, &(parameters.battery_current_scaling)) != OK) { - PX4_WARN("%s", paramerr); - - } else if (parameters.battery_current_scaling < 0.0f) { - /* apply scaling according to defaults if set to default */ - parameters.battery_current_scaling = (BOARD_ADC_POS_REF_V_FOR_CURRENT_CHAN / px4_arch_adc_dn_fullcount()); - param_set_no_notification(parameter_handles.battery_current_scaling, ¶meters.battery_current_scaling); - } - - if (param_get(parameter_handles.battery_current_offset, &(parameters.battery_current_offset)) != OK) { - PX4_WARN("%s", paramerr); - - } - - if (param_get(parameter_handles.battery_v_div, &(parameters.battery_v_div)) != OK) { - PX4_WARN("%s", paramerr); - parameters.battery_v_div = 0.0f; - - } else if (parameters.battery_v_div <= 0.0f) { - /* apply scaling according to defaults if set to default */ - - parameters.battery_v_div = BOARD_BATTERY1_V_DIV; - param_set_no_notification(parameter_handles.battery_v_div, ¶meters.battery_v_div); - } - - if (param_get(parameter_handles.battery_a_per_v, &(parameters.battery_a_per_v)) != OK) { - PX4_WARN("%s", paramerr); - parameters.battery_a_per_v = 0.0f; - - } else if (parameters.battery_a_per_v <= 0.0f) { - /* apply scaling according to defaults if set to default */ - - parameters.battery_a_per_v = BOARD_BATTERY1_A_PER_V; - param_set_no_notification(parameter_handles.battery_a_per_v, ¶meters.battery_a_per_v); - } - - param_get(parameter_handles.battery_source, &(parameters.battery_source)); - param_get(parameter_handles.battery_adc_channel, &(parameters.battery_adc_channel)); - - return ret; -} - -} /* namespace battery_status */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d7beb8976c..ef0c387056 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -693,7 +693,8 @@ protected: if (_battery_status_sub[i]->update(&_battery_status_timestamp[i], &battery_status)) { /* battery status message with higher resolution */ mavlink_battery_status_t bat_msg{}; - bat_msg.id = i; + // TODO: Determine how to better map between battery ID within the firmware and in MAVLink + bat_msg.id = battery_status.id - 1; bat_msg.battery_function = MAV_BATTERY_FUNCTION_ALL; bat_msg.type = MAV_BATTERY_TYPE_LIPO; bat_msg.current_consumed = (battery_status.connected) ? battery_status.discharged_mah : -1; diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index f3cfa44f97..177f7e38e0 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -43,8 +43,6 @@ px4_add_module( sensors.cpp 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 deleted file mode 100644 index 85ca2ae811..0000000000 --- a/src/modules/sensors/analog_battery.cpp +++ /dev/null @@ -1,141 +0,0 @@ -#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 deleted file mode 100644 index be340329a7..0000000000 --- a/src/modules/sensors/analog_battery.h +++ /dev/null @@ -1,163 +0,0 @@ -/**************************************************************************** - * - * 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 deleted file mode 100644 index de1b963545..0000000000 --- a/src/modules/sensors/power.cpp +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * 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]->getVoltageChannel() == buf_adc[i].am_channel) { - /* Voltage in ADC counts */ - bat_voltage_cnt[b] = buf_adc[i].am_data; - - } 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; - } - } - } - - /* 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(hrt_absolute_time(), bat_voltage_cnt[b], bat_current_cnt[b], - 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 deleted file mode 100644 index 06e2f44b25..0000000000 --- a/src/modules/sensors/power.h +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * 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 "analog_battery.h" - -/** - * 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: - * ``` - * AnalogBattery1 _battery0; - * AnalogBattery2 _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. - */ -#if BOARD_NUMBER_BRICKS > 0 - AnalogBattery1 _battery0; - -#if BOARD_NUMBER_BRICKS > 1 - AnalogBattery2 _battery1; -#endif - - AnalogBattery *_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/sensors.cpp b/src/modules/sensors/sensors.cpp index f69bb17647..5ba48f1cda 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -72,7 +72,6 @@ #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 badedc92dd..a492fde0fa 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -42,7 +42,7 @@ #pragma once -#include +#include #include #include #include @@ -73,6 +73,7 @@ #include #include +#include namespace simulator { @@ -237,22 +238,22 @@ private: hrt_abstime _last_sitl_timestamp{0}; hrt_abstime _last_battery_timestamp{0}; - // 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 + class : public Battery { - 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 {}; + virtual void updateParams() override + { + _params.v_empty = 3.5f; + _params.v_charged = 4.05f; + _params.n_cells = 4; + _params.capacity = 10.0f; + _params.v_load_drop = 0.0f; + _params.r_internal = 0.0f; + _params.low_thr = 0.15f; + _params.crit_thr = 0.07f; + _params.emergen_thr = 0.05f; + _params.source = 0; + } + } _battery; #ifndef __PX4_QURT diff --git a/validation/module_schema.yaml b/validation/module_schema.yaml index 31d11576a7..142724db63 100644 --- a/validation/module_schema.yaml +++ b/validation/module_schema.yaml @@ -141,7 +141,7 @@ parameters: 'celcius', 'gauss', 'gauss/s', 'mgauss', 'mgauss^2', 'hPa', 'kg', 'kg/m^2', 'mm', 'm', 'm/s', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', 'm/s/rad', - 'Ohm', + 'Ohm', 'V', 'us', 'ms', 's' ] bit: # description of all bits for type bitmask.