Skip to content

Commit

Permalink
Code cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
Timothy Scott committed Dec 19, 2019
1 parent c091d14 commit e3a30fc
Show file tree
Hide file tree
Showing 3 changed files with 52 additions and 43 deletions.
80 changes: 44 additions & 36 deletions src/drivers/power_monitor/ina226/ina226.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,8 @@ INA226::INA226(int battery_index, int bus, int address) :
index(bus),
_sample_perf(perf_alloc(PC_ELAPSED, "ina226_read")),
_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)
{
float fvalue = MAX_CURRENT;
Expand All @@ -88,9 +90,9 @@ INA226::INA226(int battery_index, int bus, int address) :
_config = (uint16_t)value;
}

_mode_trigged = ((_config & INA226_MODE_MASK) >> INA226_MODE_SHIFTS) <=
((INA226_MODE_SHUNT_BUS_TRIG & INA226_MODE_MASK) >>
INA226_MODE_SHIFTS);
_mode_triggered = ((_config & INA226_MODE_MASK) >> INA226_MODE_SHIFTS) <=
((INA226_MODE_SHUNT_BUS_TRIG & INA226_MODE_MASK) >>
INA226_MODE_SHIFTS);

_current_lsb = _max_current / DN_MAX;
_power_lsb = 25 * _current_lsb;
Expand All @@ -104,6 +106,8 @@ INA226::~INA226()
/* free perf counters */
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_collection_errors);
perf_free(_measure_errors);
}

int INA226::read(uint8_t address)
Expand Down Expand Up @@ -150,7 +154,7 @@ INA226::init()

// If we run in continuous mode then start it here

if (!_mode_trigged) {
if (!_mode_triggered) {
ret = write(INA226_REG_CONFIGURATION, _config);

} else {
Expand All @@ -169,24 +173,9 @@ INA226::init()
int
INA226::force_init()
{
_battery.updateBatteryStatus(
hrt_absolute_time(),
0.0,
0.0,
false,
false, // TODO: selected source?
0,
0.0,
false,
false
);
_battery.publish();

int ret = init();

if (ret != OK) {
start();
}
start();

return ret;
}
Expand Down Expand Up @@ -224,7 +213,7 @@ INA226::measure()
{
int ret = OK;

if (_mode_trigged) {
if (_mode_triggered) {
ret = write(INA226_REG_CONFIGURATION, _config);

if (ret < 0) {
Expand All @@ -244,10 +233,21 @@ INA226::collect()
/* read from the sensor */
perf_begin(_sample_perf);

_bus_volatage = read(INA226_REG_BUSVOLTAGE);
_power = read(INA226_REG_POWER);
_current = read(INA226_REG_CURRENT);
_shunt = read(INA226_REG_SHUNTVOLTAGE);
if (_initialized) {

_bus_voltage = read(INA226_REG_BUSVOLTAGE);
_power = read(INA226_REG_POWER);
_current = read(INA226_REG_CURRENT);
_shunt = read(INA226_REG_SHUNTVOLTAGE);

} else {
init();

_bus_voltage = -1.0f;
_power = -1.0f;
_current = -1.0f;
_shunt = -1.0f;
}

parameter_update_s param_update;

Expand All @@ -260,21 +260,19 @@ INA226::collect()
// Note: If the power module is connected backwards, then the values of _power, _current, and _shunt will
// be negative but otherwise valid. This isn't important, because why should we support the case where
// the power module is used incorrectly?
if (_bus_volatage >= 0 && _power >= 0 && _current >= 0 && _shunt >= 0) {
if (_bus_voltage >= 0 && _power >= 0 && _current >= 0 && _shunt >= 0) {

_actuators_sub.copy(&_actuator_controls);
_armed_sub.copy(&_armed);

/* publish it */
_battery.updateBatteryStatus(
hrt_absolute_time(),
(float) _bus_volatage * INA226_VSCALE,
(float) _bus_voltage * INA226_VSCALE,
(float) _current * _current_lsb,
true,
true, // TODO: Determine if this is the selected source
0,
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE],
_armed.armed,
true
);

Expand All @@ -289,7 +287,6 @@ INA226::collect()
false, // TODO: selected source?
0,
0.0,
false,
true
);
ret = -1;
Expand Down Expand Up @@ -333,14 +330,14 @@ INA226::Run()

/* perform collection */
if (OK != collect()) {
PX4_DEBUG("collection error");
perf_count(_collection_errors);
/* if error restart the measurement state machine */
start();
return;
}

/* next phase is measurement */
_collect_phase = !_mode_trigged;
_collect_phase = !_mode_triggered;

if (_measure_interval > INA226_CONVERSION_INTERVAL) {

Expand All @@ -354,7 +351,7 @@ INA226::Run()

/* Perform measurement */
if (OK != measure()) {
PX4_DEBUG("measure error ina226");
perf_count(_measure_errors);
}

/* next phase is collection */
Expand All @@ -364,9 +361,20 @@ INA226::Run()
ScheduleDelayed(INA226_CONVERSION_INTERVAL);

} else {
//PX4_INFO("Trying again to init");
init();
ScheduleDelayed(INA226_INIT_RETRY_INTERVAL_US);
_battery.updateBatteryStatus(
hrt_absolute_time(),
0.0f,
0.0f,
false,
false,
0,
0.0f,
true
);

if (init() != OK) {
ScheduleDelayed(INA226_INIT_RETRY_INTERVAL_US);
}
}
}

Expand Down
9 changes: 4 additions & 5 deletions src/drivers/power_monitor/ina226/ina226.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,6 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
Expand Down Expand Up @@ -145,13 +144,15 @@ class INA226 : public device::I2C, px4::ScheduledWorkItem, ModuleParams

perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _collection_errors;
perf_counter_t _measure_errors;

int16_t _bus_volatage{0};
int16_t _bus_voltage{0};
int16_t _power{-1};
int16_t _current{-1};
int16_t _shunt{0};
int16_t _cal{0};
bool _mode_trigged{false};
bool _mode_triggered{false};

float _max_current{MAX_CURRENT};
float _rshunt{INA226_SHUNT};
Expand All @@ -160,10 +161,8 @@ class INA226 : public device::I2C, px4::ScheduledWorkItem, ModuleParams
float _power_lsb{25.0f * _current_lsb};

actuator_controls_s _actuator_controls{};
actuator_armed_s _armed{};

Battery _battery;
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _actuators_sub{ORB_ID(actuator_controls)};
uORB::Subscription _parameters_sub{ORB_ID(parameter_update)};

Expand Down
6 changes: 4 additions & 2 deletions src/drivers/power_monitor/ina226/ina226_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,6 @@ int get_index(int i2c_bus, int address)
}
}

//PX4_INFO("No matching guy found. First nullptr: %d", first_nullptr);
return first_nullptr;
}

Expand Down Expand Up @@ -107,7 +106,7 @@ start_bus(int i2c_bus, int address, int battery_index, bool force)
}

if (force) {
if (g_dev[idx]->force_init() == OK) {
if (g_dev[idx]->force_init() != OK) {
PX4_INFO("Failed to initialize INA226 on bus %d, but will try again periodically.", i2c_bus);
}

Expand Down Expand Up @@ -198,6 +197,7 @@ this flag set, the battery must be plugged in before starting the driver.
)DESCR_STR");

PRINT_MODULE_USAGE_NAME("ina226", "driver");

PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start a new instance of the driver");
PRINT_MODULE_USAGE_PARAM_FLAG('a', "If set, try to start the driver on each availabe I2C bus until a module is found", true);
PRINT_MODULE_USAGE_PARAM_FLAG('f', "If initialization fails, keep retrying periodically. Ignored if the -a flag is set. See full driver documentation for more info", true);
Expand All @@ -206,9 +206,11 @@ this flag set, the battery must be plugged in before starting the driver.
// we can't use hexadecimal.
PRINT_MODULE_USAGE_PARAM_INT('d', 65, 0, UINT8_MAX, "I2C Address (in hexadecimal)", true);
PRINT_MODULE_USAGE_PARAM_INT('t', 1, 1, 2, "Which battery calibration values should be used (1 or 2)", true);

PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop one instance of the driver");
PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, NUM_I2C_BUS_OPTIONS - 1, "I2C bus (default: use board-specific bus)", true);
PRINT_MODULE_USAGE_PARAM_INT('d', 65, 0, UINT8_MAX, "I2C Address (in hexadecimal)", true);

PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Status of every instance of the driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("info", "Status of every instance of the driver");
}
Expand Down

0 comments on commit e3a30fc

Please sign in to comment.