dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Make all battery sensor drivers report their sample interval 

To make use of the time abstraction in the leaky integrator.
This commit is contained in:
Matthias Grob 2020-04-01 21:36:39 +02:00
parent 4dc9ca69ab
commit 88c21337fa
14 changed files with 48 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
boards/bitcraze/crazyflie/syslink/syslink_main.h
View File

@ -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<input_rc_s> _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;

2
src/drivers/power_monitor/ina226/ina226.cpp
View File

@ -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;

8
src/drivers/power_monitor/ina226/ina226.h
View File

@ -13,6 +13,8 @@
#include <uORB/topics/parameter_update.h>
#include <px4_platform_common/i2c_spi_buses.h>
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};

6
src/drivers/power_monitor/voxlpm/voxlpm.cpp
View File

@ -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

4
src/drivers/power_monitor/voxlpm/voxlpm.hpp
View File

@ -73,6 +73,8 @@
#include <uORB/topics/power_monitor.h>
#include <uORB/topics/parameter_update.h>
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<power_monitor_s> _pm_pub_topic{ORB_ID(power_monitor)};

11
src/lib/battery/battery.cpp
View File

@ -45,15 +45,18 @@
#include <cstring>
#include <px4_platform_common/defines.h>
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<float>(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);

2
src/lib/battery/battery.h
View File

@ -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();

4
src/modules/battery_status/analog_battery.cpp
View File

@ -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];

2
src/modules/battery_status/analog_battery.h
View File

@ -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.

9
src/modules/battery_status/battery_status.cpp
View File

@ -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;
}

4
src/modules/esc_battery/EscBattery.cpp
View File

@ -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;
}

3
src/modules/esc_battery/EscBattery.hpp
View File

@ -47,6 +47,8 @@
#include <uORB/topics/actuator_controls.h>
#include <battery/battery.h>
using namespace time_literals;
class EscBattery : public ModuleBase<EscBattery>, 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;
};

7
src/modules/simulator/simulator.h
View File

@ -81,6 +81,8 @@
#include <v2.0/mavlink_types.h>
#include <lib/battery/battery.h>
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
{

2
src/modules/simulator/simulator_mavlink.cpp
View File

@ -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;