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

modalai_fc-v1 - power_monitor/voxlpm: add support for voxlpm v3 (INA231 sensors)

This commit is contained in:
modaltb 2020-08-18 21:01:31 -07:00 committed by GitHub
parent 841f650be4
commit d9bbed767c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 507 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@ -33,54 +33,217 @@
/**
* @file voxlpm.cpp
* Driver for the VOXL Power Management unit
* Driver for the VOXL Power Module unit
*/
#include "voxlpm.hpp"
/*
* The VOXLPM has two LTC2946 ICs on it.
* The VOXLPM v2 has two LTC2946 ICs on it.
* Address 0x6A - measures battery voltage and current with a 0.0005 ohm sense resistor
* Address 0x6B - measures 5VDC ouptut voltage and current
* Address 0x6B - measures 5VDC ouptut voltage and current with a 0.005 ohm sense resistor
*
* The VOXLPM v3 has two INA231 ICs on it.
* Address 0x44 - measures battery voltage and current with a 0.0005 ohm sense resistor
* Address 0x45 - measures 5VDC/12VDC ouptut voltage and current with a 0.005 ohm sense resistor
*/
VOXLPM::VOXLPM(I2CSPIBusOption bus_option, const int bus, int bus_frequency, VOXLPM_CH_TYPE ch_type) :
I2C(DRV_POWER_DEVTYPE_VOXLPM, MODULE_NAME, bus,
(ch_type == VOXLPM_CH_TYPE_VBATT) ? VOXLPM_LTC2946_ADDR_VBATT : VOXLPM_LTC2946_ADDR_P5VD, bus_frequency),
I2C(DRV_POWER_DEVTYPE_VOXLPM, MODULE_NAME, bus, VOXLPM_INA231_ADDR_VBATT, 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")),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")),
_ch_type(ch_type),
_battery(1, this, _meas_interval_us)
{
if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
_rsense = VOXLPM_RSENSE_VBATT;
} else {
_rsense = VOXLPM_RSENSE_5VOUT;
}
}
VOXLPM::~VOXLPM()
{
perf_free(_sample_perf);
perf_free(_comms_errors);
}
int
VOXLPM::init()
{
_initialized = false;
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
_battery.updateBatteryStatus(
hrt_absolute_time(),
0.0,
0.0,
false,
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
0,
0.0
);
}
/* do I2C init, it will probe the bus for two possible configurations, LTC2946 or INA231 */
if (I2C::init() != OK) {
return ret;
}
write_reg(DEFAULT_CTRLA_REG_VAL, VOXLPM_LTC2946_CTRLA_REG);
write_reg(DEFAULT_CTRLB_REG_VAL, VOXLPM_LTC2946_CTRLB_REG);
/* If we've probed and succeeded we'll have an accurate address here for the VBat addr */
uint8_t addr = get_device_address();
_battery.reset();
if (addr == VOXLPM_LTC2946_ADDR_VBATT || addr == VOXLPM_LTC2946_ADDR_P5VD) {
_pm_type = VOXLPM_TYPE_V2_LTC;
load_params(_pm_type, _ch_type);
ret = init_ltc2946();
} else if (addr == VOXLPM_INA231_ADDR_VBATT || addr == VOXLPM_INA231_ADDR_P5_12VDC) {
_pm_type = VOXLPM_TYPE_V3_INA;
load_params(_pm_type, _ch_type);
ret = init_ina231();
} else {
PX4_ERR("Unkown device address");
ret = PX4_ERROR;
}
if (ret == PX4_OK) {
_initialized = true;
start();
}
return ret;
}
int
VOXLPM::force_init()
{
int ret = init();
start();
return ret;
}
int
VOXLPM::probe()
{
int ret = PX4_ERROR;
uint8_t data[2];
uint8_t addr;
/* Try LTC2946 first */
if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
addr = VOXLPM_LTC2946_ADDR_VBATT;
} else {
addr = VOXLPM_LTC2946_ADDR_P5VD;
}
set_device_address(addr);
/* Check status register */
ret = read_reg_buf(VOXLPM_LTC2946_STATUS_REG, data, sizeof(data));
if (ret) {
/* Try INA231 next */
if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
addr = VOXLPM_INA231_ADDR_VBATT;
} else {
addr = VOXLPM_INA231_ADDR_P5_12VDC;
}
set_device_address(addr);
/* Check config register */
ret = read_reg_buf(INA231_REG_CONFIG, data, sizeof(data));
}
return ret;
}
int
VOXLPM::load_params(VOXLPM_TYPE pm_type, VOXLPM_CH_TYPE ch_type)
{
if (pm_type == VOXLPM_TYPE_V2_LTC) {
/* No configuration needed */
_rshunt = (ch_type == VOXLPM_CH_TYPE_VBATT) ? VOXLPM_LTC2946_VBAT_SHUNT : VOXLPM_LTC2946_VREG_SHUNT;
} else if (pm_type == VOXLPM_TYPE_V3_INA) {
_rshunt = -1.0f;
float fvalue = -1.0f;
param_t ph;
/* Allow for configuration */
if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
ph = param_find("VOXLPM_SHUNT_BAT");
if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
_rshunt = fvalue;
}
} else {
ph = param_find("VOXLPM_SHUNT_REG");
if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
_rshunt = fvalue;
}
}
if (_rshunt < 0) {
_rshunt = (_ch_type == VOXLPM_CH_TYPE_VBATT) ? VOXLPM_INA231_VBAT_SHUNT : VOXLPM_INA231_VREG_SHUNT;
}
}
return PX4_OK;
}
int
VOXLPM::init_ltc2946()
{
write_reg(VOXLPM_LTC2946_CTRLA_REG, DEFAULT_LTC2946_CTRLA_REG_VAL);
write_reg(VOXLPM_LTC2946_CTRLB_REG, DEFAULT_LTC2946_CTRLB_REG_VAL);
return PX4_OK;
}
int
VOXLPM::init_ina231()
{
int ret = PX4_OK;
uint16_t cmd;
/* Reset */
cmd = INA231_RST_BIT;
ret = write_word_swapped(INA231_REG_CONFIG, cmd);
if (ret) {
PX4_ERR("Failed to reset INA231");
return ret;
}
/* Config */
cmd = INA231_CONFIG;
ret = write_word_swapped(INA231_REG_CONFIG, cmd);
if (ret) {
PX4_ERR("Failed to config INA231");
return ret;
}
if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
_cal = (INA231_CONST / (VOXLPM_INA231_VBAT_I_LSB * _rshunt));
} else {
_cal = (INA231_CONST / (VOXLPM_INA231_VREG_I_LSB * _rshunt));
}
/* Set calibration */
ret = write_word_swapped(INA231_REG_CALIBRATION, _cal);
if (ret) {
PX4_ERR("Failed to calibrate INA231");
return ret;
}
return PX4_OK;
}
@ -90,16 +253,41 @@ VOXLPM::print_status()
{
perf_print_counter(_sample_perf);
if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
printf("- type: BATT\n");
switch (_pm_type) {
case VOXLPM_TYPE_V2_LTC:
printf("- V2 (LTC2964)\n");
break;
} else {
printf("- type: P5VDC\n");
case VOXLPM_TYPE_V3_INA:
printf("- V3 (INA231)\n");
break;
default:
break;
}
printf(" - voltage: %9.2f VDC \n", (double)_voltage);
printf(" - current: %9.2f ADC \n", (double)_amperage);
printf(" - rsense: %9.6f ohm \n", (double)_rsense);
switch (_ch_type) {
case VOXLPM_CH_TYPE_VBATT:
printf("- type: BATT\n");
break;
case VOXLPM_CH_TYPE_P5VDC:
printf("- type: P5VDC\n");
break;
case VOXLPM_CH_TYPE_P12VDC:
printf("- type: P12VDC\n");
break;
default:
printf("- type: UNKOWN\n");
break;
}
printf(" - voltage: %9.4f VDC \n", (double)_voltage);
printf(" - current: %9.4f ADC \n", (double)_amperage);
printf(" - shunt: %9.4f mV, %9.4f mA\n", (double)_vshunt * 1000, (double)_vshuntamps * 1000);
printf(" - rsense: %9.6f ohm, cal: %i\n", (double)_rshunt, _cal);
printf(" - meas interval: %u us \n", _meas_interval_us);
}
@ -118,44 +306,58 @@ VOXLPM::RunImpl()
int
VOXLPM::measure()
{
int ret = PX4_ERROR;
if (!_initialized) {
ret = init();
if (ret) {
return ret;
}
}
parameter_update_s update;
if (_parameter_sub.update(&update)) {
updateParams();
}
perf_begin(_sample_perf);
_voltage = 0.0f;
_amperage = 0.0f;
uint8_t vraw[2];
uint8_t iraw[2];
perf_begin(_sample_perf);
hrt_abstime tnow = hrt_absolute_time();
int curr_read_ret = read_reg_buf(VOXLPM_LTC2946_DELTA_SENSE_MSB_REG, iraw, sizeof(iraw)); // 0x14
int volt_read_ret = read_reg_buf(VOXLPM_LTC2946_VIN_MSB_REG, vraw, sizeof(vraw)); // 0x1E
switch (_pm_type) {
case VOXLPM_TYPE_V2_LTC:
ret = measure_ltc2946();
break;
if ((volt_read_ret == 0) && (curr_read_ret == 0)) {
uint16_t volt16 = (((uint16_t)vraw[0]) << 8) | vraw[1]; // MSB first
volt16 >>= 4; // data is 12 bit and left-aligned
_voltage = (volt16 / VOXLPM_LTC2946_RESOLUTION) * VOXLPM_LTC2946_VFS_SENSE;
case VOXLPM_TYPE_V3_INA:
ret = measure_ina231();
break;
uint16_t curr16 = (((uint16_t)iraw[0]) << 8) | iraw[1]; // MSB first
curr16 >>= 4; // data is 12 bit and left-aligned
_amperage = curr16 / VOXLPM_LTC2946_RESOLUTION * VOXLPM_LTC2946_VFS_DELTA_SENSE / _rsense;
default:
break;
}
if (ret == PX4_OK) {
switch (_ch_type) {
case VOXLPM_CH_TYPE_VBATT: {
_battery.updateBatteryStatus(tnow, _voltage, _amperage, true,
battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0);
_actuators_sub.copy(&_actuator_controls);
_battery.updateBatteryStatus(tnow,
_voltage,
_amperage,
true,
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
0,
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]);
}
// fallthrough
case VOXLPM_CH_TYPE_P5VDC: {
case VOXLPM_CH_TYPE_P5VDC:
case VOXLPM_CH_TYPE_P12VDC: {
memset(&_pm_status, 0x00, sizeof(_pm_status));
_pm_status.timestamp = tnow;
_pm_status.voltage_v = (float) _voltage;
@ -169,10 +371,17 @@ VOXLPM::measure()
}
} else {
perf_count(_comms_errors);
switch (_ch_type) {
case VOXLPM_CH_TYPE_VBATT: {
_battery.updateBatteryStatus(tnow, 0.0, 0.0, true,
battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0);
_battery.updateBatteryStatus(tnow,
0.0,
0.0,
true,
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
0,
0.0);
}
break;
@ -183,7 +392,73 @@ VOXLPM::measure()
perf_end(_sample_perf);
return OK;
return ret;
}
int
VOXLPM::measure_ltc2946()
{
int ret = PX4_ERROR;
uint8_t vraw[2];
uint8_t iraw[2];
int amp_ret = read_reg_buf(VOXLPM_LTC2946_DELTA_SENSE_MSB_REG, iraw, sizeof(iraw)); // 0x14
int volt_ret = read_reg_buf(VOXLPM_LTC2946_VIN_MSB_REG, vraw, sizeof(vraw)); // 0x1E
if ((amp_ret == 0) && (volt_ret == 0)) {
uint16_t volt16 = (((uint16_t)vraw[0]) << 8) | vraw[1]; // MSB first
volt16 >>= 4; // data is 12 bit and left-aligned
_voltage = (volt16 / VOXLPM_LTC2946_RESOLUTION) * VOXLPM_LTC2946_VFS_SENSE;
uint16_t curr16 = (((uint16_t)iraw[0]) << 8) | iraw[1]; // MSB first
curr16 >>= 4; // data is 12 bit and left-aligned
_amperage = curr16 / VOXLPM_LTC2946_RESOLUTION * VOXLPM_LTC2946_VFS_DELTA_SENSE / _rshunt;
ret = PX4_OK;
}
return ret;
}
int
VOXLPM::measure_ina231()
{
int ret = PX4_ERROR;
uint8_t raw_vshunt[2];
uint8_t raw_vbus[2];
uint8_t raw_amps[2];
int16_t vshunt = -1;
uint16_t vbus = -1;
uint16_t amps = 0;
int vshunt_ret = read_reg_buf(INA231_REG_SHUNTVOLTAGE, raw_vshunt, sizeof(raw_vshunt));
int vbus_ret = read_reg_buf(INA231_REG_BUSVOLTAGE, raw_vbus, sizeof(raw_vbus));
int amp_ret = read_reg_buf(INA231_REG_CURRENT, raw_amps, sizeof(raw_amps));
if ((vshunt_ret == 0) && (vbus_ret == 0) && (amp_ret == 0)) {
vshunt = (((int16_t)raw_vshunt[0]) << 8) | raw_vshunt[1]; // MSB first
vbus = (((uint16_t)raw_vbus[0]) << 8) | raw_vbus[1]; // MSB first
amps = (((uint16_t)raw_amps[0]) << 8) | raw_amps[1]; // MSB first
_voltage = (float) vbus * INA231_VBUSSCALE;
_vshunt = (float) vshunt * INA231_VSHUNTSCALE;
if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
/* vshunt is in microvolts, convert to AMPs */
_vshuntamps = ((float) _vshunt / VOXLPM_INA231_VBAT_SHUNT);
_amperage = (float) amps * VOXLPM_INA231_VBAT_I_LSB;
} else {
/* vshunt is in microvolts, convert to AMPs */
_vshuntamps = ((float) _vshunt / VOXLPM_INA231_VREG_SHUNT);
_amperage = (float) amps * VOXLPM_INA231_VREG_I_LSB;
}
ret = PX4_OK;
}
return ret;
}
uint8_t
@ -191,7 +466,6 @@ VOXLPM::read_reg(uint8_t addr)
{
uint8_t cmd[2] = { (uint8_t)(addr), 0};
transfer(&cmd[0], 1, &cmd[1], 1);
return cmd[1];
}
@ -203,8 +477,18 @@ VOXLPM::read_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len)
}
int
VOXLPM::write_reg(uint8_t value, uint8_t addr)
VOXLPM::write_reg(uint8_t addr, uint8_t value)
{
uint8_t cmd[2] = { (uint8_t)(addr), value};
return transfer(cmd, sizeof(cmd), nullptr, 0);
}
int
VOXLPM::write_word_swapped(uint8_t addr, uint16_t value)
{
uint8_t data[3] = {};
data[0] = addr;
data[1] = (value & 0xFF00) >> 8;
data[2] = (value & 0x00FF);
return transfer(data, sizeof(data), nullptr, 0);
}

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

@ -34,10 +34,12 @@
/**
* @file voxlpm.hpp
*
* Shared defines for the voxlpm (QTY2 LTC2946) driver.
* Shared defines for the voxlpm driver.
*
* This is roughly what's goin on:
*
* - VOXLPM v2 (QTY2 LTC2946) -
*
* +~~~~~~~~~~~~~~+
* VBATT -----| RSENSE_VBATT | ----------+---------------------> VBATT TO ESCS
* | +~~~~~~~~~~~~~~+ |
@ -52,6 +54,24 @@
* ################# #################
* # LTC2946, 0x6a # # LTC2946, 0x6b #
* ################# #################
*
* - VOXLPM v3 (QTY2 INA231) -
*
* +~~~~~~~~~~~~~~+
* VBATT -----| RSENSE_VBATT | ----------+---------------------> VBATT TO ESCS
* | +~~~~~~~~~~~~~~+ |
* | | +--------+------+
* +----+ +----+ | 5/12V REGULATOR |
* | | +--------+------+
* | | | +~~~~~~~~~~~~~~+
* | | +---| RSENSE_5VOUT |---> 5/12VDC TO COMPUTE/PERIPHERAL
* | | | +~~~~~~~~~~~~~~+
* | | | |
* V| |A V| |A
* ################# #################
* # INA231, 0x44 # # INA231, 0x45 #
* ################# #################
*
*
* Publishes: Publishes:
* - ORB_ID(battery_status)
@ -69,6 +89,7 @@
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/power_monitor.h>
#include <uORB/topics/parameter_update.h>
@ -76,7 +97,7 @@
using namespace time_literals;
/*
* Note that these are unshifted addresses.
* VOXLPM v2 - Note that these are unshifted addresses.
*/
#define VOXLPM_LTC2946_ADDR_VBATT 0x6a // 0x6a = 0xd4 >> 1
#define VOXLPM_LTC2946_ADDR_P5VD 0x6b // 0x6b = 0xd6 >> 1
@ -87,6 +108,7 @@ using namespace time_literals;
#define VOXLPM_LTC2946_POWER_MSB2_REG 0x05
#define VOXLPM_LTC2946_CTRLB_MSG1_REG 0x06
#define VOXLPM_LTC2946_CTRLB_LSB_REG 0x07
#define VOXLPM_LTC2946_STATUS_REG 0x80
#define VOXLPM_LTC2946_DELTA_SENSE_MSB_REG 0x14
#define VOXLPM_LTC2946_DELTA_SENSE_LSB_REG 0x15
@ -106,7 +128,7 @@ using namespace time_literals;
* 2:0 - [Channel Configuration]
* 000 --> Alternate Voltage, Current Measurement
*/
#define DEFAULT_CTRLA_REG_VAL 0x18
#define DEFAULT_LTC2946_CTRLA_REG_VAL 0x18
/*
* CTRLB (Address 0x01 - LTC2946_CTRLA_REG)
@ -124,7 +146,7 @@ using namespace time_literals;
* 1:0 - [Auto-Reset Mode/Reset]
* 01 --> Enable Auto-Reset
*/
#define DEFAULT_CTRLB_REG_VAL 0x01
#define DEFAULT_LTC2946_CTRLB_REG_VAL 0x01
/* 12 bits */
#define VOXLPM_LTC2946_RESOLUTION 4095.0f
@ -136,14 +158,67 @@ using namespace time_literals;
#define VOXLPM_LTC2946_VFS_DELTA_SENSE 0.1024f
/* Power sense resistor for battery current */
#define VOXLPM_RSENSE_VBATT 0.0005f
#define VOXLPM_LTC2946_VBAT_SHUNT 0.0005f
/* Power sense resistor for 5VDC output current */
#define VOXLPM_RSENSE_5VOUT 0.005f
#define VOXLPM_LTC2946_VREG_SHUNT 0.005f
/*
* VOXLPM v3 - Coniguration from SBOS644C –FEBRUARY 2013–REVISED MARCH 2018
* http://www.ti.com/lit/ds/symlink/ina231.pdf
*/
#define VOXLPM_INA231_ADDR_VBATT 0x44
#define VOXLPM_INA231_ADDR_P5_12VDC 0x45
/* INA231 Registers addresses */
#define INA231_REG_CONFIG 0x00
#define INA231_REG_SHUNTVOLTAGE 0x01
#define INA231_REG_BUSVOLTAGE 0x02
#define INA231_REG_POWER 0x03
#define INA231_REG_CURRENT 0x04
#define INA231_REG_CALIBRATION 0x05
#define INA231_REG_MASKENABLE 0x06
#define INA231_REG_ALERTLIMIT 0x07
/* [0:2] Mode - Shunt and bus, 111, continuous (INA231A default) */
#define INA231_CONFIG_MODE (0x07 << 0)
/* [5:3] Shunt Voltage Conversion Time, 100, 1.1ms (INA231A default) */
#define INA231_CONFIG_SHUNT_CT (0x04 << 3)
/* [8:6] Shunt Voltage Conversion Time, 100, 1.1ms (INA231A default) */
#define INA231_CONFIG_BUS_CT (0x04 << 6)
/* [11:9] Averaging Mode, 010, 16 */
#define INA231_CONFIG_AVG (0x02 << 9)
/* [1] Reset bit */
#define INA231_RST_BIT (0x01 << 15)
/* Configuration register settings */
#define INA231_CONFIG (INA231_CONFIG_MODE+INA231_CONFIG_SHUNT_CT+INA231_CONFIG_BUS_CT+INA231_CONFIG_AVG)
#define INA231_CONST 0.00512f /* is an internal fixed value used to ensure scaling is maintained properly */
#define INA231_VBUSSCALE 0.00125f /* LSB of bus voltage is 1.25 mV */
#define INA231_VSHUNTSCALE 0.0000025f /* LSB of shunt voltage is 2.5 uV */
/* From SCH-M00041 REVB */
#define VOXLPM_INA231_VBAT_SHUNT 0.0005f /* VBAT shunt is 500 micro-ohm */
#define VOXLPM_INA231_VREG_SHUNT 0.005f /* VREG output shunt is 5 milli-ohm */
#define VOXLPM_INA231_VBAT_MAX_AMPS 90.0f /* 90.0 Amps max through VBAT sense resistor */
#define VOXLPM_INA231_VREG_MAX_AMPS 6.0f /* 6.0 Amps max through VREG sense resistor */
/* ina231.pdf section 8.5 */
#define VOXLPM_INA231_VBAT_I_LSB (VOXLPM_INA231_VBAT_MAX_AMPS/32768.0f)
#define VOXLPM_INA231_VREG_I_LSB (VOXLPM_INA231_VREG_MAX_AMPS/32768.0f)
#define swap16(w) __builtin_bswap16((w))
enum VOXLPM_TYPE {
VOXLPM_UNKOWN,
VOXLPM_TYPE_V2_LTC,
VOXLPM_TYPE_V3_INA
};
enum VOXLPM_CH_TYPE {
VOXLPM_CH_TYPE_VBATT = 0,
VOXLPM_CH_TYPE_P5VDC
VOXLPM_CH_TYPE_P5VDC,
VOXLPM_CH_TYPE_P12VDC
};
class VOXLPM : public device::I2C, public ModuleParams, public I2CSPIDriver<VOXLPM>
@ -157,29 +232,46 @@ public:
static void print_usage();
virtual int init();
int force_init();
void print_status() override;
void RunImpl();
private:
int probe() override;
void start();
int measure();
int load_params(VOXLPM_TYPE pm_type, VOXLPM_CH_TYPE ch_type);
int init_ltc2946();
int init_ina231();
int measure_ltc2946();
int measure_ina231();
bool _initialized;
static constexpr unsigned _meas_interval_us{100_ms};
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
uORB::PublicationMulti<power_monitor_s> _pm_pub_topic{ORB_ID(power_monitor)};
uORB::Subscription _parameter_sub{ORB_ID(parameter_update)};
power_monitor_s _pm_status{};
VOXLPM_TYPE _pm_type{VOXLPM_UNKOWN};
const VOXLPM_CH_TYPE _ch_type;
float _voltage{0.0f};
float _amperage{0.0f};
float _rsense{0.0f};
float _rshunt{0.0005f};
float _vshunt{0.0f};
float _vshuntamps{0.0f};
int16_t _cal{0};
Battery _battery;
uORB::Subscription _actuators_sub{ORB_ID(actuator_controls_0)};
actuator_controls_s _actuator_controls{};
uint8_t read_reg(uint8_t addr);
int read_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len);
int write_reg(uint8_t value, uint8_t addr);
int write_reg(uint8_t addr, uint8_t value);
int write_word_swapped(uint8_t addr, uint16_t value);
};

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

@ -47,7 +47,13 @@ I2CSPIDriverBase *VOXLPM::instantiate(const BusCLIArguments &cli, const BusInsta
return nullptr;
}
if (OK != instance->init()) {
if (cli.custom1 == 1) {
if (OK != instance->force_init()) {
PX4_INFO("Failed to init voxlpm type: %d on bus: %d, but will try again periodically.", (VOXLPM_CH_TYPE)cli.type,
iterator.bus());
}
} else if (OK != instance->init()) {
delete instance;
return nullptr;
}
@ -62,7 +68,8 @@ VOXLPM::print_usage()
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_PARAM_STRING('T', "VBATT", "VBATT|P5VDC|P12VDC", "Type", true);
PRINT_MODULE_USAGE_PARAM_FLAG('K', "if initialization (probing) fails, keep retrying periodically", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
@ -75,7 +82,7 @@ voxlpm_main(int argc, char *argv[])
cli.default_i2c_frequency = 400000;
cli.type = VOXLPM_CH_TYPE_VBATT;
while ((ch = cli.getopt(argc, argv, "T:")) != EOF) {
while ((ch = cli.getopt(argc, argv, "KT:")) != EOF) {
switch (ch) {
case 'T':
if (strcmp(cli.optarg(), "VBATT") == 0) {
@ -84,12 +91,19 @@ voxlpm_main(int argc, char *argv[])
} else if (strcmp(cli.optarg(), "P5VDC") == 0) {
cli.type = VOXLPM_CH_TYPE_P5VDC;
} else if (strcmp(cli.optarg(), "P12VDC") == 0) {
cli.type = VOXLPM_CH_TYPE_P12VDC; // same as P5VDC
} else {
PX4_ERR("unknown type");
return -1;
}
break;
case 'K': // keep retrying
cli.custom1 = 1;
break;
}
}

58
src/drivers/power_monitor/voxlpm/voxlpm_params.c Normal file
View File

@ -0,0 +1,58 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* VOXL Power Monitor Shunt, Battery
*
* @reboot_required true
*
* @min 0.000000001
* @max 0.1
* @decimal 10
* @increment .000000001
* @group Sensors
*/
PARAM_DEFINE_FLOAT(VOXLPM_SHUNT_BAT, 0.00063f);
/**
* VOXL Power Monitor Shunt, Regulator
*
* @reboot_required true
*
* @min 0.000000001
* @max 0.1
* @decimal 10
* @increment .000000001
* @group Sensors
*/
PARAM_DEFINE_FLOAT(VOXLPM_SHUNT_REG, 0.0056f);