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

refactor voxlpm: use driver base class

This commit is contained in:
Beat Küng 2020-03-03 19:01:38 +01:00 committed by Daniel Agar
parent 832ccd262e
commit 692d262e0e
5 changed files with 69 additions and 181 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
boards/modalai/fc-v1/init/rc.board_sensors
View File

@ -6,7 +6,8 @@
adc start
# Start Digital power monitors
voxlpm -R start
voxlpm -X -b 3 -T VBATT start
voxlpm -X -b 3 -T P5VDC start
# Internal SPI bus ICM-20602
mpu6000 -R 6 -s -T 20602 start

1
src/drivers/drv_sensor.h
View File

@ -137,6 +137,7 @@
#define DRV_DIST_DEVTYPE_TERARANGER 0x75
#define DRV_DIST_DEVTYPE_VL53LXX 0x76
#define DRV_POWER_DEVTYPE_INA226 0x77
#define DRV_POWER_DEVTYPE_VOXLPM 0x78
#define DRV_DEVTYPE_UNUSED 0xff

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

@ -43,15 +43,15 @@
* Address 0x6A - measures battery voltage and current with a 0.0005 ohm sense resistor
* Address 0x6B - measures 5VDC ouptut voltage and current
*/
VOXLPM::VOXLPM(const char *path, int bus, int address, VOXLPM_CH_TYPE ch_type) :
I2C("voxlpm", path, bus, address, 400000),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(I2C::get_device_id())),
VOXLPM::VOXLPM(I2CSPIBusOption bus_option, const int bus, int bus_frequency, VOXLPM_CH_TYPE ch_type) :
I2C("voxlpm", nullptr, bus, (ch_type == VOXLPM_CH_TYPE_VBATT) ? VOXLPM_LTC2946_ADDR_VBATT : VOXLPM_LTC2946_ADDR_P5VD,
bus_frequency),
ModuleParams(nullptr),
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)
{
_ch_type = ch_type;
if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
_rsense = VOXLPM_RSENSE_VBATT;
@ -62,8 +62,6 @@ VOXLPM::VOXLPM(const char *path, int bus, int address, VOXLPM_CH_TYPE ch_type) :
VOXLPM::~VOXLPM()
{
// make sure we are truly inactive
stop();
perf_free(_sample_perf);
}
@ -88,7 +86,7 @@ VOXLPM::init()
}
void
VOXLPM::print_info()
VOXLPM::print_status()
{
perf_print_counter(_sample_perf);
@ -108,22 +106,11 @@ VOXLPM::print_info()
void
VOXLPM::start()
{
/* make sure we are stopped first */
uint32_t last_meas_interval = _meas_interval;
stop();
_meas_interval = last_meas_interval;
ScheduleOnInterval(_meas_interval, 1000);
}
void
VOXLPM::stop()
{
ScheduleClear();
}
void
VOXLPM::Run()
VOXLPM::RunImpl()
{
measure();
}

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

@ -63,7 +63,7 @@
#include <drivers/device/i2c.h>
#include <perf/perf_counter.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
#include <battery/battery.h>
@ -144,22 +144,25 @@ enum VOXLPM_CH_TYPE {
VOXLPM_CH_TYPE_P5VDC
};
class VOXLPM : public device::I2C, public px4::ScheduledWorkItem, public ModuleParams
class VOXLPM : public device::I2C, public ModuleParams, public I2CSPIDriver<VOXLPM>
{
public:
VOXLPM(const char *path, int bus, int address, VOXLPM_CH_TYPE ch_type);
VOXLPM(I2CSPIBusOption bus_option, const int bus, int bus_frequency, VOXLPM_CH_TYPE ch_type);
virtual ~VOXLPM();
virtual int init();
void print_info();
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
virtual int init();
void print_status() override;
void RunImpl();
private:
unsigned _meas_interval{100000}; // 100ms
void Run() override;
void start();
void stop();
int measure();
static constexpr unsigned _meas_interval{100000}; // 100ms
perf_counter_t _sample_perf;
uORB::PublicationMulti<power_monitor_s> _pm_pub_topic{ORB_ID(power_monitor)};
@ -167,7 +170,7 @@ private:
power_monitor_s _pm_status{};
VOXLPM_CH_TYPE _ch_type;
const VOXLPM_CH_TYPE _ch_type;
float _voltage{0.0f};
float _amperage{0.0f};
float _rsense{0.0f};

198
src/drivers/power_monitor/voxlpm/voxlpm_main.cpp
View File

@ -36,188 +36,84 @@
#include "voxlpm.hpp"
enum VOXLPM_BUS {
VOXLPM_BUS_I2C_EXTERNAL,
VOXLPM_BUS_I2C_EXTERNAL1,
VOXLPM_BUS_I2C_EXTERNAL2
};
/**
* Local functions in support of the shell command.
*/
namespace voxlpm
I2CSPIDriverBase *VOXLPM::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
struct voxlpm_chan {
const char *devpath;
uint32_t device;
VOXLPM *dev;
};
VOXLPM *instance = new VOXLPM(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency,
(VOXLPM_CH_TYPE)cli.type);
struct voxlpm_bus_option {
enum VOXLPM_BUS busid;
uint8_t busnum;
bool external;
struct voxlpm_chan vbat;
struct voxlpm_chan vpwr;
} bus_options[] = {
#if defined(PX4_I2C_BUS_EXPANSION)
{
VOXLPM_BUS_I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION, true,
{"/dev/voxlpm_vbat", VOXLPM_LTC2946_ADDR_VBATT, NULL },
{"/dev/voxlpm_p5vd", VOXLPM_LTC2946_ADDR_P5VD, NULL}
},
#endif
#if defined(PX4_I2C_BUS_EXPANSION1)
{
VOXLPM_BUS_I2C_EXTERNAL1, PX4_I2C_BUS_EXPANSION1, true,
{"/dev/voxlpm_vbat", VOXLPM_LTC2946_ADDR_VBATT, NULL },
{"/dev/voxlpm_p5vd", VOXLPM_LTC2946_ADDR_P5VD, NULL}
},
#endif
#if defined(PX4_I2C_BUS_EXPANSION2)
{
VOXLPM_BUS_I2C_EXTERNAL2, PX4_I2C_BUS_EXPANSION2, true,
{"/dev/voxlpm_vbat", VOXLPM_LTC2946_ADDR_VBATT, NULL },
{"/dev/voxlpm_p5vd", VOXLPM_LTC2946_ADDR_P5VD, NULL}
},
#endif
};
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
/**
* Start the driver.
*/
bool
start_bus(struct voxlpm_bus_option &bus)
{
/* assume if we've got the battery channel we have the output channel as well */
if (bus.vbat.dev != nullptr) {
PX4_ERR("bus option already started");
exit(1);
if (instance == nullptr) {
PX4_ERR("alloc failed");
return nullptr;
}
/* create the battery voltage / current channel */
bus.vbat.dev = new VOXLPM(bus.vbat.devpath, bus.busnum, bus.vbat.device, VOXLPM_CH_TYPE_VBATT);
if (bus.vbat.dev == nullptr) {
return false;
if (OK != instance->init()) {
delete instance;
return nullptr;
}
/* create the 5VDC output / compute current channel */
bus.vpwr.dev = new VOXLPM(bus.vpwr.devpath, bus.busnum, bus.vpwr.device, VOXLPM_CH_TYPE_P5VDC);
if (bus.vbat.dev == nullptr) {
return false;
}
if (bus.vbat.dev->init() != OK || bus.vpwr.dev->init() != OK) {
delete bus.vbat.dev;
bus.vbat.dev = nullptr;
delete bus.vpwr.dev;
bus.vpwr.dev = nullptr;
return false;
}
return true;
return instance;
}
void
start(enum VOXLPM_BUS busid)
VOXLPM::print_usage()
{
bool started = false;
uint8_t i;
PRINT_MODULE_USAGE_NAME_SIMPLE("voxlpm", "driver");
for (i = 0; i < NUM_BUS_OPTIONS; i++) {
if (bus_options[i].busid == busid) {
started = start_bus(bus_options[i]);
break; // one bus only...
}
}
if (!started) {
PX4_WARN("bus option number is %d", i);
PX4_ERR("driver start failed");
exit(1);
}
exit(0);
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAM_STRING('T', "VBATT", "VBATT|P5VDC", "Type", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
void
info()
extern "C" int
voxlpm_main(int argc, char *argv[])
{
uint8_t i;
for (i = 0; i < NUM_BUS_OPTIONS; i++) {
if (bus_options[i].vbat.dev) {
bus_options[i].vbat.dev->print_info();
}
if (bus_options[i].vpwr.dev) {
bus_options[i].vpwr.dev->print_info();
}
}
exit(0);
}
void
usage()
{
PRINT_MODULE_USAGE_NAME_SIMPLE("voxlpm", "command");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "start monitoring");
PRINT_MODULE_USAGE_COMMAND_DESCR("info", "display info");;
PRINT_MODULE_USAGE_COMMAND_DESCR("-X", "PX4_I2C_BUS_EXPANSION");
PRINT_MODULE_USAGE_COMMAND_DESCR("-T", "PX4_I2C_BUS_EXPANSION1");
PRINT_MODULE_USAGE_COMMAND_DESCR("-R", "PX4_I2C_BUS_EXPANSION2 (default)");
}
} // namespace
extern "C" __EXPORT int voxlpm_main(int argc, char *argv[])
{
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
enum VOXLPM_BUS busid = VOXLPM_BUS_I2C_EXTERNAL2;
using ThisDriver = VOXLPM;
BusCLIArguments cli{true, false};
cli.default_i2c_frequency = 400000;
cli.type = VOXLPM_CH_TYPE_VBATT;
while ((ch = px4_getopt(argc, argv, "XTR", &myoptind, &myoptarg)) != EOF) {
while ((ch = cli.getopt(argc, argv, "T:")) != EOF) {
switch (ch) {
case 'X':
busid = VOXLPM_BUS_I2C_EXTERNAL;
break;
case 'T':
busid = VOXLPM_BUS_I2C_EXTERNAL1;
break;
if (strcmp(cli.optarg(), "VBATT") == 0) {
cli.type = VOXLPM_CH_TYPE_VBATT;
case 'R':
busid = VOXLPM_BUS_I2C_EXTERNAL2;
break;
} else if (strcmp(cli.optarg(), "P5VDC") == 0) {
cli.type = VOXLPM_CH_TYPE_P5VDC;
default:
voxlpm::usage();
return 0;
} else {
PX4_ERR("unknown type");
return -1;
}
break;
}
}
if (myoptind >= argc) {
voxlpm::usage();
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
const char *verb = argv[myoptind];
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_POWER_DEVTYPE_VOXLPM);
if (!strcmp(verb, "start")) {
voxlpm::start(busid);
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "info")) {
voxlpm::info();
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
voxlpm::usage();
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}