diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.h b/boards/bitcraze/crazyflie/syslink/syslink_main.h index 5e5952dd06..fc28a1b40d 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.h +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.h @@ -48,6 +48,8 @@ #include "syslink.h" #include "crtp.h" +using namespace time_literals; + #define MIN(X, Y) (((X) < (Y)) ? (X) : (Y)) @@ -137,7 +139,9 @@ private: uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; - Battery _battery{1, nullptr}; + // 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}; 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 d013ea62a0..97fdd6789f 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -49,7 +49,7 @@ INA226::INA226(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int _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) + _battery(battery_index, this, INA226_SAMPLE_INTERVAL_US) { float fvalue = MAX_CURRENT; _max_current = fvalue; diff --git a/src/drivers/power_monitor/ina226/ina226.h b/src/drivers/power_monitor/ina226/ina226.h index 6f4740752d..06f2c8846e 100644 --- a/src/drivers/power_monitor/ina226/ina226.h +++ b/src/drivers/power_monitor/ina226/ina226.h @@ -13,6 +13,8 @@ #include #include +using namespace time_literals; + /* Configuration Constants */ #define INA226_BASEADDR 0x41 /* 7-bit address. 8-bit address is 0x41 */ // If initialization is forced (with the -f flag on the command line), but it fails, the drive will try again to @@ -98,7 +100,9 @@ #define INA226_SUL (1 << 14) #define INA226_SOL (1 << 15) -#define INA226_CONVERSION_INTERVAL (100000-7) /* 100 ms / 10 Hz */ +#define INA226_SAMPLE_FREQUENCY_HZ 10 +#define INA226_SAMPLE_INTERVAL_US (1_s / INA226_SAMPLE_FREQUENCY_HZ) +#define INA226_CONVERSION_INTERVAL (INA226_SAMPLE_INTERVAL_US - 7) #define MAX_CURRENT 164.0f /* 164 Amps */ #define DN_MAX 32768.0f /* 2^15 */ #define INA226_CONST 0.00512f /* is an internal fixed value used to ensure scaling is maintained properly */ @@ -139,7 +143,7 @@ protected: private: bool _sensor_ok{false}; - int _measure_interval{0}; + unsigned _measure_interval{0}; bool _collect_phase{false}; bool _initialized{false}; diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.cpp b/src/drivers/power_monitor/voxlpm/voxlpm.cpp index b610e28ece..da5a532a55 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.cpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.cpp @@ -50,7 +50,7 @@ VOXLPM::VOXLPM(I2CSPIBusOption bus_option, const int bus, int bus_frequency, VOX I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")), _ch_type(ch_type), - _battery(1, this) + _battery(1, this, _meas_interval_us) { if (_ch_type == VOXLPM_CH_TYPE_VBATT) { _rsense = VOXLPM_RSENSE_VBATT; @@ -100,13 +100,13 @@ VOXLPM::print_status() printf(" - voltage: %9.2f VDC \n", (double)_voltage); printf(" - current: %9.2f ADC \n", (double)_amperage); printf(" - rsense: %9.6f ohm \n", (double)_rsense); - printf(" - meas interval: %u us \n", _meas_interval); + printf(" - meas interval: %u us \n", _meas_interval_us); } void VOXLPM::start() { - ScheduleOnInterval(_meas_interval, 1000); + ScheduleOnInterval(_meas_interval_us, 1000); } void diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.hpp b/src/drivers/power_monitor/voxlpm/voxlpm.hpp index f1a3f93b65..b010ed9167 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.hpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.hpp @@ -73,6 +73,8 @@ #include #include +using namespace time_literals; + /* * Note that these are unshifted addresses. */ @@ -162,7 +164,7 @@ private: void start(); int measure(); - static constexpr unsigned _meas_interval{100000}; // 100ms + static constexpr unsigned _meas_interval_us{100_ms}; perf_counter_t _sample_perf; uORB::PublicationMulti _pm_pub_topic{ORB_ID(power_monitor)}; diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 53b8199ee2..ecc15e3bd4 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -45,15 +45,18 @@ #include #include -Battery::Battery(int index, ModuleParams *parent) : +using namespace time_literals; + +Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) : ModuleParams(parent), _index(index < 1 || index > 9 ? 1 : index), _warning(battery_status_s::BATTERY_WARNING_NONE), _last_timestamp(0) { - _voltage_filter_v.setParameters(.01f, 1.f); - _current_filter_a.setParameters(.01f, .5f); - _throttle_filter.setParameters(.01f, 1.f); + const float expected_filter_dt = static_cast(sample_interval_us) / 1_s; + _voltage_filter_v.setParameters(expected_filter_dt, 1.f); + _current_filter_a.setParameters(expected_filter_dt, .5f); + _throttle_filter.setParameters(expected_filter_dt, 1.f); if (index > 9 || index < 1) { PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index); diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 5b32bf7f1f..8d4145318a 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -64,7 +64,7 @@ class Battery : public ModuleParams { public: - Battery(int index, ModuleParams *parent); + Battery(int index, ModuleParams *parent, const int sample_interval_us); ~Battery(); diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index 17e4075757..a2e2835875 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -15,8 +15,8 @@ static constexpr int DEFAULT_V_CHANNEL[1] = {0}; static constexpr int DEFAULT_I_CHANNEL[1] = {0}; #endif -AnalogBattery::AnalogBattery(int index, ModuleParams *parent) : - Battery(index, parent) +AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_interval_us) : + Battery(index, parent, sample_interval_us) { char param_name[17]; diff --git a/src/modules/battery_status/analog_battery.h b/src/modules/battery_status/analog_battery.h index 35c7d9ca50..42b4e2c3fa 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); + AnalogBattery(int index, ModuleParams *parent, const int sample_interval_us); /** * Update current battery status message. diff --git a/src/modules/battery_status/battery_status.cpp b/src/modules/battery_status/battery_status.cpp index 453d6e4e26..b7df0bae56 100644 --- a/src/modules/battery_status/battery_status.cpp +++ b/src/modules/battery_status/battery_status.cpp @@ -103,6 +103,9 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ uORB::Subscription _adc_report_sub{ORB_ID(adc_report)}; + static constexpr uint32_t SAMPLE_FREQUENCY_HZ = 100; + static constexpr uint32_t SAMPLE_INTERVAL_US = 1_s / SAMPLE_FREQUENCY_HZ; + AnalogBattery _battery1; #if BOARD_NUMBER_BRICKS > 1 @@ -135,9 +138,9 @@ private: BatteryStatus::BatteryStatus() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), - _battery1(1, this), + _battery1(1, this, SAMPLE_INTERVAL_US), #if BOARD_NUMBER_BRICKS > 1 - _battery2(2, this), + _battery2(2, this, SAMPLE_INTERVAL_US), #endif _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME)) { @@ -277,7 +280,7 @@ BatteryStatus::task_spawn(int argc, char *argv[]) bool BatteryStatus::init() { - ScheduleOnInterval(10_ms); // 100 Hz + ScheduleOnInterval(SAMPLE_INTERVAL_US); return true; } diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index 71a7199780..0eeb8934e7 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) + _battery(1, this, ESC_BATTERY_INTERVAL_US) { } @@ -50,6 +50,8 @@ EscBattery::init() return false; } + _esc_status_sub.set_interval_us(ESC_BATTERY_INTERVAL_US); + return true; } diff --git a/src/modules/esc_battery/EscBattery.hpp b/src/modules/esc_battery/EscBattery.hpp index 60a53e300c..0c2a6cc8b1 100644 --- a/src/modules/esc_battery/EscBattery.hpp +++ b/src/modules/esc_battery/EscBattery.hpp @@ -47,6 +47,8 @@ #include #include +using namespace time_literals; + class EscBattery : public ModuleBase, public ModuleParams, public px4::WorkItem { public: @@ -73,5 +75,6 @@ private: uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; uORB::SubscriptionCallbackWorkItem _esc_status_sub{this, ORB_ID(esc_status)}; + static constexpr uint32_t ESC_BATTERY_INTERVAL_US = 20_ms; // assume higher frequency esc feedback than 50Hz Battery _battery; }; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index acbf068b09..da8b5708ca 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -81,6 +81,8 @@ #include #include +using namespace time_literals; + //! Enumeration to use on the bitmask in HIL_SENSOR enum class SensorSource { ACCEL = 0b111, @@ -193,7 +195,10 @@ private: class SimulatorBattery : public Battery { public: - SimulatorBattery() : Battery(1, nullptr) {} + static constexpr uint32_t SIMLATOR_BATTERY_SAMPLE_FREQUENCY_HZ = 100; // Hz + static constexpr uint32_t SIMLATOR_BATTERY_SAMPLE_INTERVAL_US = 1_s / SIMLATOR_BATTERY_SAMPLE_FREQUENCY_HZ; + + SimulatorBattery() : Battery(1, nullptr, SIMLATOR_BATTERY_SAMPLE_INTERVAL_US) {} virtual void updateParams() override { diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 5499bdd4a6..0922caaf30 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -350,7 +350,7 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) static uint64_t last_integration_us = 0; // battery simulation (limit update to 100Hz) - if (hrt_elapsed_time(&_last_battery_timestamp) >= 10_ms) { + if (hrt_elapsed_time(&_last_battery_timestamp) >= SimulatorBattery::SIMLATOR_BATTERY_SAMPLE_INTERVAL_US) { const float discharge_interval_us = _param_sim_bat_drain.get() * 1000 * 1000;