forked from Archive/PX4-Autopilot
batt_smbus: use driver base class
This commit is contained in:
Beat Küng
Daniel Agar
parent
ec2de33547
commit
82f92b56db
|
|
@ -47,8 +47,8 @@
|
|||
|
||||
extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]);
|
||||
|
||||
BATT_SMBUS::BATT_SMBUS(SMBus *interface, const char *path) :
|
||||
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
|
||||
BATT_SMBUS::BATT_SMBUS(I2CSPIBusOption bus_option, const int bus, SMBus *interface) :
|
||||
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus),
|
||||
_interface(interface)
|
||||
{
|
||||
battery_status_s new_report = {};
|
||||
|
|
@ -81,79 +81,8 @@ BATT_SMBUS::~BATT_SMBUS()
|
|||
param_set(param_find("BAT_SOURCE"), &battsource);
|
||||
}
|
||||
|
||||
int BATT_SMBUS::task_spawn(int argc, char *argv[])
|
||||
void BATT_SMBUS::RunImpl()
|
||||
{
|
||||
enum BATT_SMBUS_BUS busid = BATT_SMBUS_BUS_ALL;
|
||||
|
||||
int myoptind = 1;
|
||||
int ch;
|
||||
const char *myoptarg = nullptr;
|
||||
|
||||
while ((ch = px4_getopt(argc, argv, "XTRIA", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'X':
|
||||
busid = BATT_SMBUS_BUS_I2C_EXTERNAL;
|
||||
break;
|
||||
|
||||
case 'T':
|
||||
busid = BATT_SMBUS_BUS_I2C_EXTERNAL1;
|
||||
break;
|
||||
|
||||
case 'R':
|
||||
busid = BATT_SMBUS_BUS_I2C_EXTERNAL2;
|
||||
break;
|
||||
|
||||
case 'I':
|
||||
busid = BATT_SMBUS_BUS_I2C_INTERNAL;
|
||||
break;
|
||||
|
||||
case 'A':
|
||||
busid = BATT_SMBUS_BUS_ALL;
|
||||
break;
|
||||
|
||||
default:
|
||||
print_usage();
|
||||
return PX4_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
|
||||
|
||||
if (!is_running() && (busid == BATT_SMBUS_BUS_ALL || smbus_bus_options[i].busid == busid)) {
|
||||
|
||||
SMBus *interface = new SMBus(smbus_bus_options[i].busnum, BATT_SMBUS_ADDR);
|
||||
BATT_SMBUS *dev = new BATT_SMBUS(interface, smbus_bus_options[i].devpath);
|
||||
|
||||
int result = dev->get_startup_info();
|
||||
|
||||
if (result != PX4_OK) {
|
||||
delete dev;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Successful read of device type, we've found our battery
|
||||
_object.store(dev);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
dev->ScheduleOnInterval(BATT_SMBUS_MEASUREMENT_INTERVAL_US);
|
||||
|
||||
return PX4_OK;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
PX4_WARN("Not found.");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
void BATT_SMBUS::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
// Get the current time.
|
||||
uint64_t now = hrt_absolute_time();
|
||||
|
||||
|
|
@ -354,7 +283,7 @@ int BATT_SMBUS::dataflash_read(uint16_t &address, void *data, const unsigned len
|
|||
return result;
|
||||
}
|
||||
|
||||
int BATT_SMBUS::dataflash_write(uint16_t &address, void *data, const unsigned length)
|
||||
int BATT_SMBUS::dataflash_write(uint16_t address, void *data, const unsigned length)
|
||||
{
|
||||
uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS;
|
||||
|
||||
|
|
@ -387,7 +316,7 @@ int BATT_SMBUS::get_startup_info()
|
|||
result = manufacturer_name((uint8_t *)man_name, sizeof(man_name));
|
||||
|
||||
if (result != PX4_OK) {
|
||||
PX4_WARN("Failed to get manufacturer name");
|
||||
PX4_DEBUG("Failed to get manufacturer name");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
|
|
@ -559,96 +488,7 @@ int BATT_SMBUS::lifetime_read_block_one()
|
|||
return PX4_OK;
|
||||
}
|
||||
|
||||
int BATT_SMBUS::custom_command(int argc, char *argv[])
|
||||
{
|
||||
const char *input = argv[0];
|
||||
uint8_t man_name[22];
|
||||
int result = 0;
|
||||
|
||||
if (!is_running()) {
|
||||
PX4_ERR("not running");
|
||||
return -1;
|
||||
}
|
||||
|
||||
BATT_SMBUS *obj = get_instance();
|
||||
|
||||
if (!strcmp(input, "man_info")) {
|
||||
|
||||
result = obj->manufacturer_name(man_name, sizeof(man_name));
|
||||
PX4_INFO("The manufacturer name: %s", man_name);
|
||||
|
||||
result = obj->manufacture_date();
|
||||
PX4_INFO("The manufacturer date: %d", result);
|
||||
|
||||
uint16_t serial_num = 0;
|
||||
serial_num = obj->get_serial_number();
|
||||
PX4_INFO("The serial number: %d", serial_num);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(input, "unseal")) {
|
||||
obj->unseal();
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(input, "seal")) {
|
||||
obj->seal();
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(input, "suspend")) {
|
||||
obj->suspend();
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(input, "resume")) {
|
||||
obj->resume();
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(input, "serial_num")) {
|
||||
uint16_t serial_num = obj->get_serial_number();
|
||||
PX4_INFO("Serial number: %d", serial_num);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(input, "write_flash")) {
|
||||
if (argc >= 3) {
|
||||
uint16_t address = atoi(argv[1]);
|
||||
unsigned length = atoi(argv[2]);
|
||||
uint8_t tx_buf[32] = {};
|
||||
|
||||
if (length > 32) {
|
||||
PX4_WARN("Data length out of range: Max 32 bytes");
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Data needs to be fed in 1 byte (0x01) at a time.
|
||||
for (unsigned i = 0; i < length; i++) {
|
||||
if ((unsigned)argc <= 3 + i) {
|
||||
tx_buf[i] = atoi(argv[3 + i]);
|
||||
}
|
||||
}
|
||||
|
||||
if (PX4_OK != obj->dataflash_write(address, tx_buf, length)) {
|
||||
PX4_INFO("Dataflash write failed: %d", address);
|
||||
px4_usleep(100000);
|
||||
return 1;
|
||||
|
||||
} else {
|
||||
px4_usleep(100000);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
print_usage();
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int BATT_SMBUS::print_usage()
|
||||
void BATT_SMBUS::print_usage()
|
||||
{
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
|
|
@ -664,11 +504,8 @@ $ batt_smbus -X write_flash 19069 2 27 0
|
|||
PRINT_MODULE_USAGE_NAME("batt_smbus", "driver");
|
||||
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAM_FLAG('X', "BATT_SMBUS_BUS_I2C_EXTERNAL", true);
|
||||
PRINT_MODULE_USAGE_PARAM_FLAG('T', "BATT_SMBUS_BUS_I2C_EXTERNAL1", true);
|
||||
PRINT_MODULE_USAGE_PARAM_FLAG('R', "BATT_SMBUS_BUS_I2C_EXTERNAL2", true);
|
||||
PRINT_MODULE_USAGE_PARAM_FLAG('I', "BATT_SMBUS_BUS_I2C_INTERNAL", true);
|
||||
PRINT_MODULE_USAGE_PARAM_FLAG('A', "BATT_SMBUS_BUS_ALL", true);
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x0B);
|
||||
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("man_info", "Prints manufacturer info.");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("unseal", "Unseals the devices flash memory to enable write_flash commands.");
|
||||
|
|
@ -681,11 +518,151 @@ $ batt_smbus -X write_flash 19069 2 27 0
|
|||
PRINT_MODULE_USAGE_ARG("number of bytes", "Number of bytes to send.", true);
|
||||
PRINT_MODULE_USAGE_ARG("data[0]...data[n]", "One byte of data at a time separated by spaces.", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int batt_smbus_main(int argc, char *argv[])
|
||||
I2CSPIDriverBase *BATT_SMBUS::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance)
|
||||
{
|
||||
return BATT_SMBUS::main(argc, argv);
|
||||
SMBus *interface = new SMBus(iterator.bus(), cli.i2c_address);
|
||||
if (interface == nullptr) {
|
||||
PX4_ERR("alloc failed");
|
||||
return nullptr;
|
||||
}
|
||||
BATT_SMBUS *instance = new BATT_SMBUS(iterator.configuredBusOption(), iterator.bus(), interface);
|
||||
|
||||
if (instance == nullptr) {
|
||||
PX4_ERR("alloc failed");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
int result = instance->get_startup_info();
|
||||
|
||||
if (result != PX4_OK) {
|
||||
delete instance;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
instance->ScheduleOnInterval(BATT_SMBUS_MEASUREMENT_INTERVAL_US);
|
||||
|
||||
return instance;
|
||||
}
|
||||
|
||||
void
|
||||
BATT_SMBUS::custom_method(const BusCLIArguments &cli)
|
||||
{
|
||||
switch(cli.custom1) {
|
||||
case 1: {
|
||||
uint8_t man_name[22];
|
||||
int result = manufacturer_name(man_name, sizeof(man_name));
|
||||
PX4_INFO("The manufacturer name: %s", man_name);
|
||||
|
||||
result = manufacture_date();
|
||||
PX4_INFO("The manufacturer date: %d", result);
|
||||
|
||||
uint16_t serial_num = 0;
|
||||
serial_num = get_serial_number();
|
||||
PX4_INFO("The serial number: %d", serial_num);
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
unseal();
|
||||
break;
|
||||
case 3:
|
||||
seal();
|
||||
break;
|
||||
case 4:
|
||||
suspend();
|
||||
break;
|
||||
case 5:
|
||||
resume();
|
||||
break;
|
||||
case 6:
|
||||
if (cli.custom_data) {
|
||||
unsigned address = cli.custom2;
|
||||
uint8_t *tx_buf = (uint8_t*)cli.custom_data;
|
||||
unsigned length = tx_buf[0];
|
||||
|
||||
if (PX4_OK != dataflash_write(address, tx_buf+1, length)) {
|
||||
PX4_ERR("Dataflash write failed: %d", address);
|
||||
}
|
||||
px4_usleep(100000);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[])
|
||||
{
|
||||
using ThisDriver = BATT_SMBUS;
|
||||
BusCLIArguments cli{true, false};
|
||||
cli.default_i2c_frequency = 100000;
|
||||
cli.i2c_address = BATT_SMBUS_ADDR;
|
||||
|
||||
const char *verb = cli.parseDefaultArguments(argc, argv);
|
||||
if (!verb) {
|
||||
ThisDriver::print_usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BAT_DEVTYPE_SMBUS);
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
return ThisDriver::module_start(cli, iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "stop")) {
|
||||
return ThisDriver::module_stop(iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "status")) {
|
||||
return ThisDriver::module_status(iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "man_info")) {
|
||||
cli.custom1 = 1;
|
||||
return ThisDriver::module_custom_method(cli, iterator, false);
|
||||
}
|
||||
if (!strcmp(verb, "unseal")) {
|
||||
cli.custom1 = 2;
|
||||
return ThisDriver::module_custom_method(cli, iterator);
|
||||
}
|
||||
if (!strcmp(verb, "seal")) {
|
||||
cli.custom1 = 3;
|
||||
return ThisDriver::module_custom_method(cli, iterator);
|
||||
}
|
||||
if (!strcmp(verb, "suspend")) {
|
||||
cli.custom1 = 4;
|
||||
return ThisDriver::module_custom_method(cli, iterator);
|
||||
}
|
||||
if (!strcmp(verb, "resume")) {
|
||||
cli.custom1 = 5;
|
||||
return ThisDriver::module_custom_method(cli, iterator);
|
||||
}
|
||||
if (!strcmp(verb, "write_flash")) {
|
||||
cli.custom1 = 6;
|
||||
if (argc >= 3) {
|
||||
uint16_t address = atoi(argv[1]);
|
||||
unsigned length = atoi(argv[2]);
|
||||
uint8_t tx_buf[33];
|
||||
cli.custom_data = &tx_buf;
|
||||
|
||||
if (length > 32) {
|
||||
PX4_WARN("Data length out of range: Max 32 bytes");
|
||||
return 1;
|
||||
}
|
||||
|
||||
tx_buf[0] = length;
|
||||
// Data needs to be fed in 1 byte (0x01) at a time.
|
||||
for (unsigned i = 0; i < length; i++) {
|
||||
if ((unsigned)argc <= 3 + i) {
|
||||
tx_buf[i+1] = atoi(argv[3 + i]);
|
||||
}
|
||||
}
|
||||
cli.custom2 = address;
|
||||
return ThisDriver::module_custom_method(cli, iterator);
|
||||
}
|
||||
}
|
||||
|
||||
ThisDriver::print_usage();
|
||||
return -1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -49,10 +49,10 @@
|
|||
#include <perf/perf_counter.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <px4_platform_common/i2c_spi_buses.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
|
||||
#include "board_config.h"
|
||||
#include <board_config.h>
|
||||
|
||||
#define MAC_DATA_BUFFER_SIZE 32
|
||||
|
||||
|
|
@ -95,51 +95,22 @@
|
|||
#define BATT_SMBUS_ENABLED_PROTECTIONS_A_DEFAULT 0xcf
|
||||
#define BATT_SMBUS_ENABLED_PROTECTIONS_A_CUV_DISABLED 0xce
|
||||
|
||||
#define NUM_BUS_OPTIONS (sizeof(smbus_bus_options)/sizeof(smbus_bus_options[0]))
|
||||
|
||||
enum BATT_SMBUS_BUS {
|
||||
BATT_SMBUS_BUS_ALL = 0,
|
||||
BATT_SMBUS_BUS_I2C_INTERNAL,
|
||||
BATT_SMBUS_BUS_I2C_EXTERNAL,
|
||||
BATT_SMBUS_BUS_I2C_EXTERNAL1,
|
||||
BATT_SMBUS_BUS_I2C_EXTERNAL2
|
||||
};
|
||||
|
||||
struct batt_smbus_bus_option {
|
||||
enum BATT_SMBUS_BUS busid;
|
||||
const char *devpath;
|
||||
uint8_t busnum;
|
||||
} const smbus_bus_options[] = {
|
||||
{ BATT_SMBUS_BUS_I2C_EXTERNAL, "/dev/batt_smbus_ext", PX4_I2C_BUS_EXPANSION},
|
||||
#ifdef PX4_I2C_BUS_EXPANSION1
|
||||
{ BATT_SMBUS_BUS_I2C_EXTERNAL1, "/dev/batt_smbus_ext1", PX4_I2C_BUS_EXPANSION1},
|
||||
#endif
|
||||
#ifdef PX4_I2C_BUS_EXPANSION2
|
||||
{ BATT_SMBUS_BUS_I2C_EXTERNAL2, "/dev/batt_smbus_ext2", PX4_I2C_BUS_EXPANSION2},
|
||||
#endif
|
||||
#ifdef PX4_I2C_BUS_ONBOARD
|
||||
{ BATT_SMBUS_BUS_I2C_INTERNAL, "/dev/batt_smbus_int", PX4_I2C_BUS_ONBOARD},
|
||||
#endif
|
||||
};
|
||||
|
||||
class BATT_SMBUS : public ModuleBase<BATT_SMBUS>, public px4::ScheduledWorkItem
|
||||
class BATT_SMBUS : public I2CSPIDriver<BATT_SMBUS>
|
||||
{
|
||||
public:
|
||||
|
||||
BATT_SMBUS(SMBus *interface, const char *path);
|
||||
BATT_SMBUS(I2CSPIBusOption bus_option, const int bus, SMBus *interface);
|
||||
|
||||
~BATT_SMBUS();
|
||||
|
||||
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance);
|
||||
static void print_usage();
|
||||
|
||||
friend SMBus;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage();
|
||||
void RunImpl();
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
void custom_method(const BusCLIArguments &cli) override;
|
||||
|
||||
/**
|
||||
* @brief Reads data from flash.
|
||||
|
|
@ -156,7 +127,7 @@ public:
|
|||
* @param length The number of bytes being written.
|
||||
* @return Returns PX4_OK on success, PX4_ERROR on failure.
|
||||
*/
|
||||
int dataflash_write(uint16_t &address, void *data, const unsigned length);
|
||||
int dataflash_write(uint16_t address, void *data, const unsigned length);
|
||||
|
||||
/**
|
||||
* @brief Returns the SBS serial number of the battery device.
|
||||
|
|
@ -244,8 +215,6 @@ public:
|
|||
|
||||
private:
|
||||
|
||||
void Run() override;
|
||||
|
||||
SMBus *_interface;
|
||||
|
||||
perf_counter_t _cycle{perf_alloc(PC_ELAPSED, "batt_smbus_cycle")};
|
||||
|
|
|
|||
|
|
@ -141,6 +141,7 @@
|
|||
#define DRV_LED_DEVTYPE_BLINKM 0x79
|
||||
#define DRV_LED_DEVTYPE_RGBLED 0x7a
|
||||
#define DRV_LED_DEVTYPE_RGBLED_NCP5623C 0x7b
|
||||
#define DRV_BAT_DEVTYPE_SMBUS 0x7c
|
||||
|
||||
#define DRV_DEVTYPE_UNUSED 0xff
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue