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

refactor ina226: use driver base class

This commit is contained in:
Beat Küng 2020-03-06 10:54:21 +01:00 committed by Daniel Agar
parent d6b9cfa3e2
commit 832ccd262e
5 changed files with 73 additions and 309 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
boards/px4/fmu-v5x/init/rc.board_sensors
View File

@ -6,8 +6,8 @@
adc start
# Start Digital power monitors
ina226 -b 0 -t 1 -f start
ina226 -b 1 -t 2 -f start
ina226 -X -b 1 -t 1 -k start
ina226 -X -b 2 -t 2 -k start
# Internal SPI bus ICM-20602
mpu6000 -R 8 -s -T 20602 start

1
src/drivers/drv_sensor.h
View File

@ -136,6 +136,7 @@
#define DRV_DIST_DEVTYPE_SRF02 0x74
#define DRV_DIST_DEVTYPE_TERARANGER 0x75
#define DRV_DIST_DEVTYPE_VL53LXX 0x76
#define DRV_POWER_DEVTYPE_INA226 0x77
#define DRV_DEVTYPE_UNUSED 0xff

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

@ -37,29 +37,14 @@
*
* Driver for the I2C attached INA226
*/
#define INA226_RAW // remove this
#include "ina226.h"
#include <string.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <drivers/device/i2c.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_hrt.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/power_monitor.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
INA226::INA226(int battery_index, int bus, int address) :
I2C("INA226", nullptr, i2c_bus_options[bus], address, 100000),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
INA226::INA226(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address, int battery_index) :
I2C("INA226", nullptr, bus, address, bus_frequency),
ModuleParams(nullptr),
index(bus),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address),
_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")),
@ -112,9 +97,6 @@ INA226::INA226(int battery_index, int bus, int address) :
INA226::~INA226()
{
/* make sure we are truly inactive */
stop();
/* free perf counters */
perf_free(_sample_perf);
perf_free(_comms_errors);
@ -173,8 +155,6 @@ INA226::init()
ret = OK;
}
set_device_address(INA226_BASEADDR); /* set I2c Address */
start();
_sensor_ok = true;
@ -329,13 +309,7 @@ INA226::start()
}
void
INA226::stop()
{
ScheduleClear();
}
void
INA226::Run()
INA226::RunImpl()
{
if (_initialized) {
if (_collect_phase) {
@ -391,8 +365,10 @@ INA226::Run()
}
void
INA226::print_info()
INA226::print_status()
{
I2CSPIDriverBase::print_status();
if (_initialized) {
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
@ -401,6 +377,6 @@ INA226::print_info()
} else {
PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.",
INA226_INIT_RETRY_INTERVAL_US);
INA226_INIT_RETRY_INTERVAL_US / 1000);
}
}

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

@ -1,8 +1,6 @@
#pragma once
#include <string.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <drivers/device/i2c.h>
@ -13,10 +11,9 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
/* Configuration Constants */
#define INA226_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
#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
// connect to the INA226 every this many microseconds
@ -110,13 +107,19 @@
#define swap16(w) __builtin_bswap16((w))
class INA226 : public device::I2C, px4::ScheduledWorkItem, ModuleParams
class INA226 : public device::I2C, public ModuleParams, public I2CSPIDriver<INA226>
{
public:
INA226(int battery_index, int bus = INA226_BUS_DEFAULT, int address = INA226_BASEADDR);
INA226(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address, int battery_index);
virtual ~INA226();
virtual int init() override;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
void RunImpl();
int init() override;
/**
* Tries to call the init() function. If it fails, then it will schedule to retry again in
@ -129,12 +132,10 @@ public:
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
size_t index;
void print_status() override;
protected:
virtual int probe() override;
int probe() override;
private:
bool _sensor_ok{false};
@ -166,6 +167,7 @@ private:
uORB::Subscription _actuators_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _parameters_sub{ORB_ID(parameter_update)};
/**
* Test whetpower_monitorhe device supported by the driver is present at a
* specific address.
@ -184,17 +186,6 @@ private:
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override;
int measure();
int collect();

302
src/drivers/power_monitor/ina226/ina226_main.cpp
View File

@ -1,194 +1,35 @@
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <drivers/device/i2c.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_hrt.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/power_monitor.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include "ina226.h"
#define MAX_I2C_BATTERY_COUNT 2
extern "C" __EXPORT int ina226_main(int argc, char *argv[]);
/**
* Local functions in support of the shell command.
*/
namespace ina226
I2CSPIDriverBase *INA226::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
INA226 *instance = new INA226(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address,
cli.custom2);
INA226 *g_dev[MAX_I2C_BATTERY_COUNT] = {nullptr};
int start(int i2c_bus, int address);
int start_bus(int i2c_bus, int address, int battery_index, bool force);
int stop(int i2c_bus, int address);
int info(int i2c_bus, int address);
/**
* If a device is already running on the specified bus and address, return the index of that device.
* If not, return the index of the first empty slot (nullptr) in the array.
* @param i2c_bus The bus, as an index in i2c_bus_options
* @param address I2C address of the power monitor
* @return If there is already a device running on the given bus with the given address: Return the index of that device
* If there is not already a device running: Return the index of the first nullptr in the array.
* If there are no empty slots in the array, return -1.
*/
int get_index(int i2c_bus, int address)
{
int first_nullptr = -1;
for (size_t i = 0; i < MAX_I2C_BATTERY_COUNT; i++) {
//PX4_INFO("Checking number %lu", i);
if (g_dev[i] == nullptr) {
if (first_nullptr < 0) {
first_nullptr = i;
}
} else if (g_dev[i]->get_device_bus() == i2c_bus_options[i2c_bus] && g_dev[i]->get_device_address() == address) {
return i;
}
if (instance == nullptr) {
PX4_ERR("alloc failed");
return nullptr;
}
return first_nullptr;
}
/**
*
* Attempt to start driver on all available I2C busses.
*
* This function will return as soon as the first sensor
* is detected on one of the available busses or if no
* sensors are detected.
*
*/
int
start()
{
for (unsigned i2c_bus = 0; i2c_bus < NUM_I2C_BUS_OPTIONS; i2c_bus++) {
int index = get_index(i2c_bus, INA226_BASEADDR);
if (index < 0) {
PX4_ERR("There are already %d instances of INA226 running. No more can be instantiated.",
MAX_I2C_BATTERY_COUNT);
return PX4_ERROR;
} else if (g_dev[index] == nullptr && start_bus(i2c_bus, INA226_BASEADDR, 1, false) == PX4_OK) {
return PX4_OK;
}
}
return PX4_ERROR;
}
/**
* Start the driver on a specific bus.
*
* This function only returns if the sensor is up and running
* or could not be detected successfully.
*/
int
start_bus(int i2c_bus, int address, int battery_index, bool force)
{
int idx = get_index(i2c_bus, address);
if (idx < 0) {
PX4_ERR("There are already %d instances of INA226 running. No more can be instantiated.",
MAX_I2C_BATTERY_COUNT);
return PX4_ERROR;
}
if (g_dev[idx] != nullptr) {
PX4_ERR("Already started on bus %d, address 0x%02X", i2c_bus, address);
return PX4_ERROR;
}
/* create the driver */
// TODO: Possibly change this to use statically-allocated memory and placement-new?
g_dev[idx] = new INA226(battery_index, i2c_bus, address);
if (g_dev[idx] == nullptr) {
PX4_ERR("Unable to allocate memory for INA226");
goto fail;
}
if (force) {
if (g_dev[idx]->force_init() != OK) {
PX4_INFO("Failed to initialize INA226 on bus %d, but will try again periodically.", i2c_bus);
if (cli.custom1 == 1) {
if (instance->force_init() != OK) {
PX4_INFO("Failed to init INA226 on bus %d, but will try again periodically.", iterator.bus());
}
} else if (OK != g_dev[idx]->init()) {
PX4_ERR("Failed to initialize on bus %d, address 0x%02X", i2c_bus, address);
goto fail;
} else {
PX4_INFO("Started INA226 on bus %d", i2c_bus);
} else if (OK != instance->init()) {
delete instance;
return nullptr;
}
return PX4_OK;
fail:
if (g_dev[idx] != nullptr) {
delete g_dev[idx];
g_dev[idx] = nullptr;
}
return PX4_ERROR;
return instance;
}
/**
* Stop the driver
*/
int
stop(int bus, int address)
{
int idx = get_index(bus, address);
if (idx < 0 || g_dev[idx] == nullptr) {
PX4_ERR("Driver not running on bus %d, address 0x%02X", bus, address);
return PX4_ERROR;
} else {
delete g_dev[idx];
g_dev[idx] = nullptr;
return PX4_OK;
}
}
/**
* Print a little info about the driver.
*/
int
info(int bus, int address)
{
bool any_running = false;
for (int i = 0; i < MAX_I2C_BATTERY_COUNT; i++) {
if (g_dev[i] != nullptr) {
any_running = true;
PX4_INFO("Bus %d, address 0x%02X:", (int) g_dev[i]->index, g_dev[i]->get_device_address());
g_dev[i]->print_info();
}
}
if (!any_running) {
PX4_INFO("No devices are running.");
}
return PX4_OK;
}
} /* namespace */
static void
ina2262_usage()
void
INA226::print_usage()
{
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
@ -208,101 +49,56 @@ this flag set, the battery must be plugged in before starting the driver.
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);
PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, NUM_I2C_BUS_OPTIONS - 1, "I2C bus (default: use board-specific bus)", true);
// The module documentation system INSISTS on decimal literals here. So we can't use a #define constant, and
// we can't use hexadecimal.
PRINT_MODULE_USAGE_PARAM_INT('d', 65, 0, UINT8_MAX, "I2C Address (Start with '0x' for 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 (Start with '0x' for 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");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x41);
PRINT_MODULE_USAGE_PARAM_FLAG('k', "if initialization (probing) fails, keep retrying periodically", true);
PRINT_MODULE_USAGE_PARAM_INT('t', 1, 1, 2, "battery index for calibration values (1 or 2)", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
int
extern "C" int
ina226_main(int argc, char *argv[])
{
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
bool start_all = false;
bool force = false;
using ThisDriver = INA226;
BusCLIArguments cli{true, false};
cli.i2c_address = INA226_BASEADDR;
cli.default_i2c_frequency = 100000;
cli.custom2 = 1;
int i2c_bus = INA226_BUS_DEFAULT;
int address = INA226_BASEADDR;
int battery = 1;
while ((ch = px4_getopt(argc, argv, "afb:d:t:", &myoptind, &myoptarg)) != EOF) {
while ((ch = cli.getopt(argc, argv, "kt:")) != EOF) {
switch (ch) {
case 'k': // keep retrying
cli.custom1 = 1;
break;
case 'b':
i2c_bus = atoi(myoptarg);
break;
case 'a':
start_all = true;
break;
case 'f':
force = true;
break;
case 'd':
address = strtol(myoptarg, nullptr, 0);
break;
case 't':
battery = atoi(myoptarg);
break;
default:
PX4_WARN("Unknown option!");
goto out_error;
case 't': // battery index
cli.custom2 = (int)strtol(cli.optarg(), NULL, 0);
break;
}
}
if (myoptind >= argc) {
goto out_error;
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
if (address > 255 || address < 0) {
PX4_ERR("Address must be between 0 and 255");
goto out_error;
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_POWER_DEVTYPE_INA226);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
if (start_all) {
return ina226::start();
} else {
return ina226::start_bus(i2c_bus, address, battery, force);
}
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
return ina226::stop(i2c_bus, address);
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
/*
* Print driver information.
*/
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
return ina226::info(i2c_bus, address);
}
out_error:
ina2262_usage();
return PX4_ERROR;
ThisDriver::print_usage();
return -1;
}