diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index 1ea66ca5f9..04e5157860 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp @@ -404,9 +404,7 @@ Syslink::handle_message(syslink_message_t *msg) memcpy(&vbat, &msg->data[1], sizeof(float)); //memcpy(&iset, &msg->data[5], sizeof(float)); - _battery.updateBatteryStatus(t, vbat, -1, true, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0); - + _battery.updateBatteryStatus(t, vbat, -1, true, 0); // Update battery charge state if (charging) { diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.h b/boards/bitcraze/crazyflie/syslink/syslink_main.h index 875aa0347d..a54859508d 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.h +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.h @@ -139,7 +139,7 @@ private: // nrf chip schedules battery updates with SYSLINK_SEND_PERIOD_MS static constexpr uint32_t SYSLINK_BATTERY_STATUS_INTERVAL_US = 10_ms; - Battery _battery{1, nullptr, SYSLINK_BATTERY_STATUS_INTERVAL_US}; + Battery _battery{1, nullptr, SYSLINK_BATTERY_STATUS_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE}; int32_t _rssi; battery_state _bstate; diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index 2ad6ea4b06..c53fd69483 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -49,7 +49,7 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) : _comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")), _collection_errors(perf_alloc(PC_COUNT, "ina226_collection_err")), _measure_errors(perf_alloc(PC_COUNT, "ina226_measurement_err")), - _battery(battery_index, this, INA226_SAMPLE_INTERVAL_US) + _battery(battery_index, this, INA226_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) { float fvalue = MAX_CURRENT; _max_current = fvalue; @@ -88,7 +88,6 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) : 0.0, 0.0, false, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0 ); } @@ -235,7 +234,6 @@ INA226::collect() (float) _bus_voltage * INA226_VSCALE, (float) _current * _current_lsb, success, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0 ); @@ -305,7 +303,6 @@ INA226::RunImpl() 0.0f, 0.0f, false, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0 ); diff --git a/src/drivers/power_monitor/ina228/ina228.cpp b/src/drivers/power_monitor/ina228/ina228.cpp index 71d9a19379..a45b99479e 100644 --- a/src/drivers/power_monitor/ina228/ina228.cpp +++ b/src/drivers/power_monitor/ina228/ina228.cpp @@ -49,7 +49,7 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) : _comms_errors(perf_alloc(PC_COUNT, "ina228_com_err")), _collection_errors(perf_alloc(PC_COUNT, "ina228_collection_err")), _measure_errors(perf_alloc(PC_COUNT, "ina228_measurement_err")), - _battery(battery_index, this, INA228_SAMPLE_INTERVAL_US) + _battery(battery_index, this, INA228_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) { float fvalue = MAX_CURRENT; _max_current = fvalue; @@ -90,7 +90,6 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) : 0.0, 0.0, false, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0 ); } @@ -316,7 +315,6 @@ INA228::collect() (float) _bus_voltage * INA228_VSCALE, (float) _current * _current_lsb, success, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0 ); @@ -386,7 +384,6 @@ INA228::RunImpl() 0.0f, 0.0f, false, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0 ); diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp index 3d6211521f..a6cf3acde7 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -45,7 +45,7 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) : _sample_perf(perf_alloc(PC_ELAPSED, "ina238_read")), _comms_errors(perf_alloc(PC_COUNT, "ina238_com_err")), _collection_errors(perf_alloc(PC_COUNT, "ina238_collection_err")), - _battery(battery_index, this, INA238_SAMPLE_INTERVAL_US) + _battery(battery_index, this, INA238_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) { float fvalue = DEFAULT_MAX_CURRENT; _max_current = fvalue; @@ -73,7 +73,6 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) : 0.0, 0.0, false, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0 ); } @@ -203,7 +202,6 @@ int INA238::collect() (float) bus_voltage * INA238_VSCALE, (float) current * _current_lsb, success, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0 ); @@ -264,7 +262,6 @@ void INA238::RunImpl() 0.0f, 0.0f, false, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0 ); diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.cpp b/src/drivers/power_monitor/voxlpm/voxlpm.cpp index 6827446da8..0a604dd71d 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.cpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.cpp @@ -54,7 +54,7 @@ VOXLPM::VOXLPM(const I2CSPIDriverConfig &config) : _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")), _ch_type((VOXLPM_CH_TYPE)config.custom1), - _battery(1, this, _meas_interval_us) + _battery(1, this, _meas_interval_us, battery_status_s::BATTERY_SOURCE_POWER_MODULE) { } @@ -76,7 +76,6 @@ VOXLPM::init() 0.0, 0.0, false, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0 ); } @@ -349,7 +348,6 @@ VOXLPM::measure() _voltage, _amperage, true, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0); } @@ -377,7 +375,6 @@ VOXLPM::measure() 0.0, 0.0, true, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0); } break; diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index b1bab046fc..c368c4c2b9 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -47,9 +47,10 @@ using namespace time_literals; -Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) : +Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, const uint8_t source) : ModuleParams(parent), - _index(index < 1 || index > 9 ? 1 : index) + _index(index < 1 || index > 9 ? 1 : index), + _source(source) { const float expected_filter_dt = static_cast(sample_interval_us) / 1_s; _voltage_filter_v.setParameters(expected_filter_dt, 1.f); @@ -97,7 +98,7 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) } void Battery::updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, float current_a, bool connected, - int source, int priority) + int priority) { if (!_battery_initialized) { _voltage_filter_v.reset(voltage_v); @@ -134,13 +135,13 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, battery_status.temperature = NAN; battery_status.cell_count = _params.n_cells; battery_status.connected = connected; - battery_status.source = source; + battery_status.source = _source; battery_status.priority = priority; battery_status.capacity = _params.capacity > 0.f ? static_cast(_params.capacity) : 0; battery_status.id = static_cast(_index); battery_status.warning = _warning; - if (source == _params.source) { + if (_source == _params.source) { battery_status.timestamp = hrt_absolute_time(); _battery_status_pub.publish(battery_status); } diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 41ad4888d5..eb151d75d2 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -67,7 +67,7 @@ class Battery : public ModuleParams { public: - Battery(int index, ModuleParams *parent, const int sample_interval_us); + Battery(int index, ModuleParams *parent, const int sample_interval_us, const uint8_t source); ~Battery() = default; /** @@ -91,11 +91,10 @@ public: * @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 source: Source type in relation to BAT%d_SOURCE param. * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417 */ void updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, float current_a, bool connected, - int source, int priority); + int priority); protected: struct { @@ -139,6 +138,7 @@ private: uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)}; uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; + const uint8_t _source{}; bool _battery_initialized{false}; AlphaFilter _voltage_filter_v; AlphaFilter _current_filter_a; diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index 8d10725eb4..d4cd97e762 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -49,8 +49,8 @@ static constexpr int DEFAULT_V_CHANNEL[1] = {0}; static constexpr int DEFAULT_I_CHANNEL[1] = {0}; #endif -AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_interval_us) : - Battery(index, parent, sample_interval_us) +AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_interval_us, const uint8_t source) : + Battery(index, parent, sample_interval_us, source) { char param_name[17]; @@ -71,7 +71,7 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_i void AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw, - int source, int priority) + int priority) { float voltage_v = voltage_raw * _analog_params.v_div; float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v; @@ -81,7 +81,7 @@ AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, Battery::updateBatteryStatus(timestamp, voltage_v, current_a, connected, - source, priority); + priority); } bool AnalogBattery::is_valid() diff --git a/src/modules/battery_status/analog_battery.h b/src/modules/battery_status/analog_battery.h index f51af72748..fc64720210 100644 --- a/src/modules/battery_status/analog_battery.h +++ b/src/modules/battery_status/analog_battery.h @@ -39,7 +39,7 @@ class AnalogBattery : public Battery { public: - AnalogBattery(int index, ModuleParams *parent, const int sample_interval_us); + AnalogBattery(int index, ModuleParams *parent, const int sample_interval_us, const uint8_t source); /** * Update current battery status message. @@ -51,7 +51,7 @@ public: * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417 */ void updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw, - int source, int priority); + int priority); /** * Whether the ADC channel for the voltage of this battery is valid. diff --git a/src/modules/battery_status/battery_status.cpp b/src/modules/battery_status/battery_status.cpp index e7ba2bdef0..18984919c1 100644 --- a/src/modules/battery_status/battery_status.cpp +++ b/src/modules/battery_status/battery_status.cpp @@ -137,9 +137,9 @@ private: BatteryStatus::BatteryStatus() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), - _battery1(1, this, SAMPLE_INTERVAL_US), + _battery1(1, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE), #if BOARD_NUMBER_BRICKS > 1 - _battery2(2, this, SAMPLE_INTERVAL_US), + _battery2(2, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE), #endif _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME)) { @@ -222,7 +222,6 @@ BatteryStatus::adc_poll() hrt_absolute_time(), bat_voltage_adc_readings[b], bat_current_adc_readings[b], - battery_status_s::BATTERY_SOURCE_POWER_MODULE, b ); } diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index 9dee8ac49f..755bf8248f 100644 --- a/src/modules/esc_battery/EscBattery.cpp +++ b/src/modules/esc_battery/EscBattery.cpp @@ -38,7 +38,7 @@ using namespace time_literals; EscBattery::EscBattery() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::lp_default), - _battery(1, this, ESC_BATTERY_INTERVAL_US) + _battery(1, this, ESC_BATTERY_INTERVAL_US, battery_status_s::BATTERY_SOURCE_ESCS) { } @@ -107,7 +107,6 @@ EscBattery::Run() average_voltage_v, total_current_a, connected, - battery_status_s::BATTERY_SOURCE_ESCS, priority); } } diff --git a/src/modules/simulator/battery_simulator/BatterySimulator.cpp b/src/modules/simulator/battery_simulator/BatterySimulator.cpp index 9a1b0ac11d..b4334a7b4e 100644 --- a/src/modules/simulator/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulator/battery_simulator/BatterySimulator.cpp @@ -36,7 +36,7 @@ BatterySimulator::BatterySimulator() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), - _battery(1, this, BATTERY_SIMLATOR_SAMPLE_INTERVAL_US) + _battery(1, this, BATTERY_SIMLATOR_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) { } @@ -99,7 +99,7 @@ void BatterySimulator::Run() float vbatt = math::gradual(_battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), _battery.full_cell_voltage()); vbatt *= _battery.cell_count(); - _battery.updateBatteryStatus(now_us, vbatt, ibatt, true, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0); + _battery.updateBatteryStatus(now_us, vbatt, ibatt, true, 0); perf_end(_loop_perf); }