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

resolved merge conflicts

This commit is contained in:
Jake Dahl 2018-07-25 15:16:40 -06:00 committed by Lorenz Meier
parent 79ba6b0d39
commit 3f550bf408
5 changed files with 1409 additions and 764 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/drivers/batt_smbus/CMakeLists.txt
View File

@ -35,6 +35,8 @@ px4_add_module(
MAIN batt_smbus
COMPILE_FLAGS
SRCS
batt_smbus_main.cpp
batt_smbus.cpp
batt_smbus_i2c.cpp
DEPENDS
)

1188
src/drivers/batt_smbus/batt_smbus.cpp
View File

File diff suppressed because it is too large Load Diff

322
src/drivers/batt_smbus/batt_smbus.h Normal file
View File

@ -0,0 +1,322 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 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.
*
****************************************************************************/
/**
* @file batt_smbus.h
*
* Header for a battery monitor connected via SMBus (I2C).
* Designed for BQ40Z50-R1/R2
*
* @author Randy Mackay <rmackay9@yahoo.com>
* @author Alex Klimaj <alexklimaj@gmail.com>
* @author Mark Sauder <mcsauder@gmail.com>
* @author Jacob Dahl <dahl.jakejacob@gmail.com>
*/
#include <px4_config.h>
#include <px4_workqueue.h>
#include <stdio.h>
#include <string.h>
#include <ecl/geo/geo.h>
#include <drivers/device/CDev.hpp>
#include <drivers/device/Device.hpp>
#include <drivers/device/i2c.h>
#include <drivers/drv_device.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/battery_status.h>
#define DATA_BUFFER_SIZE 32
#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION
#define BATT_SMBUS_CURRENT 0x0A ///< current register
#define BATT_SMBUS_AVERAGE_CURRENT 0x0B ///< current register
#define BATT_SMBUS_ADDR 0x0B ///< Default 7 bit address I2C address. 8 bit = 0x16
#define BATT_SMBUS_ADDR_MIN 0x00 ///< lowest possible address
#define BATT_SMBUS_ADDR_MAX 0xFF ///< highest possible address
#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC
#define BATT_SMBUS_TEMP 0x08 ///< temperature register
#define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register
#define BATT_SMBUS_FULL_CHARGE_CAPACITY 0x10 ///< capacity when fully charged
#define BATT_SMBUS_RUN_TIME_TO_EMPTY 0x11 ///< predicted remaining battery capacity based on the present rate of discharge in min
#define BATT_SMBUS_AVERAGE_TIME_TO_EMPTY 0x12 ///< predicted remaining battery capacity based on the present rate of discharge in min
#define BATT_SMBUS_REMAINING_CAPACITY 0x0F ///< predicted remaining battery capacity as a percentage
#define BATT_SMBUS_CYCLE_COUNT 0x17 ///< number of cycles the battery has experienced
#define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register
#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register
#define BATT_SMBUS_MANUFACTURER_NAME 0x20 ///< manufacturer name
#define BATT_SMBUS_MANUFACTURE_DATE 0x1B ///< manufacture date register
#define BATT_SMBUS_SERIAL_NUMBER 0x1C ///< serial number register
#define BATT_SMBUS_MEASUREMENT_INTERVAL_US 100000 ///< time in microseconds, measure at 10Hz
#define BATT_SMBUS_TIMEOUT_US 1000000 ///< timeout looking for battery 10seconds after startup
#define BATT_SMBUS_MANUFACTURER_ACCESS 0x00
#define BATT_SMBUS_MANUFACTURER_DATA 0x23
#define BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS 0x44
#define BATT_SMBUS_SECURITY_KEYS 0x0035
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
/**
* @brief Nuttshell accessible method to return the driver usage arguments.
*/
void batt_smbus_usage();
/**
* @brief Nuttshell accessible method to return the battery manufacture date.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int manufacture_date();
/**
* @brief Nuttshell accessible method to return the battery manufacturer name.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int manufacturer_name();
/**
* @brief Nuttshell accessible method to return the battery serial number.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int serial_number();
extern device::Device *BATT_SMBUS_I2C_interface(int bus);
typedef device::Device *(*BATT_SMBUS_constructor)(int);
class BATT_SMBUS : public device::CDev
{
public:
/**
* @brief Default Constructor.
* @param interface The device communication interface (i2c)
* @param path The device i2c address
*/
BATT_SMBUS(device::Device *interface, const char *path);
/**
* @brief Default Destructor.
*/
virtual ~BATT_SMBUS();
/**
* @brief Sends a block read command.
* @param cmd_code The command code.
* @param data The returned data.
* @param length The number of bytes being read
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int block_read(const uint8_t cmd_code, void *data, const unsigned length);
/**
* @brief Sends a block write command.
* @param cmd_code The command code.
* @param data The data to be written.
* @param length The number of bytes being written.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int block_write(const uint8_t cmd_code, void *data, const unsigned length);
/**
* @brief Reads data from flash.
* @param address The address to start the read from.
* @param data The returned data.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int dataflash_read(uint16_t &address, void *data);
/**
* @brief Writes data to flash.
* @param address The start address of the write.
* @param data The data to be written.
* @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);
/**
* @brief Calculates the PEC from the data.
* @param buff The buffer that stores the data.
* @param length The number of bytes being written.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
uint8_t get_pec(uint8_t *buffer, const uint8_t length);
/**
* @brief Returns the SBS serial number of the battery device.
* @return Returns the SBS serial number of the battery device.
*/
uint16_t get_serial_number();
/**
* @brief Read info from battery on startup.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int get_startup_info();
/**
* @brief Prints the latest report.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int info();
/**
* @brief Initializes the smart battery device. Calls probe() to check for device on bus.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
virtual int init();
/**
* @brief Gets the SBS manufacture date of the battery.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int manufacture_date(void *man_date);
/**
* @brief Gets the SBS manufacturer name of the battery device.
* @param manufacturer_name Pointer to a buffer into which the manufacturer name is to be written.
* @param max_length The maximum number of bytes to attempt to read from the manufacturer name register,
* including the null character that is appended to the end.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int manufacturer_name(uint8_t *manufacturer_name, const uint8_t length);
/**
* @brief Performs a ManufacturerBlockAccess() read command.
* @param cmd_code The command code.
* @param data The returned data.
* @param length The number of bytes being written.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int manufacturer_read(const uint16_t cmd_code, void *data, const unsigned length);
/**
* @brief Performs a ManufacturerBlockAccess() write command.
* @param cmd_code The command code.
* @param data The sent data.
* @param length The number of bytes being written.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int manufacturer_write(const uint16_t cmd_code, void *data, const unsigned length);
/**
* @brief Sends a read-word command with cmd_code as the command.
* @param cmd_code The command code.
* @param data The returned data.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int read_word(const uint8_t cmd_code, void *data);
/**
* @brief Search all possible slave addresses for a smart battery.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int search_addresses();
/**
* @brief Starts periodic reads from the battery.
*/
void start();
/**
* @brief Stops periodic reads from the battery.
*/
void stop();
/**
* @brief Unseals the battery to allow writing to restricted flash.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int unseal();
protected:
Device *_interface;
private:
/**
* @brief Static function that is called by worker queue.
*/
static void cycle_trampoline(void *arg);
/**
* @brief The loop that continually generates new reports.
*/
void cycle();
/** @struct _work Work queue for scheduling reads. */
work_s _work = work_s{};
/** @param _last_report Last published report, used for test(). */
battery_status_s _last_report = battery_status_s{};
/** @param _batt_topic uORB battery topic. */
orb_advert_t _batt_topic;
/** @param _batt_orb_id uORB battery topic ID. */
orb_id_t _batt_orb_id;
/** @param _batt_capacity Battery design capacity in mAh (0 means unknown). */
uint16_t _batt_capacity;
/** @param _batt_startup_capacity Battery remaining capacity in mAh on startup. */
uint16_t _batt_startup_capacity;
/** @param _cycle_count The number of cycles the battery has experienced. */
uint16_t _cycle_count;
/** @param _serial_number Serial number register. */
uint16_t _serial_number;
/** @param _crit_thr Critical battery threshold param. */
float _crit_thr;
/** @param _emergency_thr Emergency battery threshold param. */
float _emergency_thr;
/** @param _low_thr Low battery threshold param. */
float _low_thr;
/** @param _manufacturer_name Name of the battery manufacturer. */
char *_manufacturer_name;
/* Do not allow copy construction or move assignment of this class. */
BATT_SMBUS(const BATT_SMBUS &);
BATT_SMBUS operator=(const BATT_SMBUS &);
};

131
src/drivers/batt_smbus/batt_smbus_i2c.cpp Normal file
View File

@ -0,0 +1,131 @@
/****************************************************************************
*
* Copyright (c) 2018 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.
*
****************************************************************************/
/**
* @file batt_smbus_i2c.cpp
*
* I2C interface for batt_smbus
*
* @author Jacob Dahl <dahl.jakejacob@gmail.com>
*/
#include "batt_smbus.h"
#include <cstring>
#include <px4_config.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_device.h>
#include "board_config.h"
device::Device *BATT_SMBUS_I2C_interface(int bus);
class BATT_SMBUS_I2C : public device::I2C
{
public:
BATT_SMBUS_I2C(int bus);
virtual ~BATT_SMBUS_I2C() = default;
/**
* @brief Sends a block read command.
* @param cmd_code The command code.
* @param data Pointer to the data being returned.
* @param count The number of bytes being read
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
virtual int read(unsigned cmd_code, void *data, unsigned count);
/**
* @brief Sends a block write command.
* @param cmd_code The command code.
* @param data Pointer to the data to be written.
* @param count The number of bytes being written.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
virtual int write(unsigned cmd_code, void *data, unsigned count);
/**
* @brief Sends a block read command.
* @param cmd_code The command code.
* @param data Pointer to the data being returned.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int read(unsigned cmd_code, void *data);
/**
* @brief Sends a block write command.
* @param cmd_code The command code.
* @param data Pointer to the data to be written.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
int write(unsigned cmd_code, void *data);
protected:
/**
* @brief Calculates the PEC from the data.
* @param buffer The buffer that stores the data.
* @param length The number of bytes being written.
* @return Returns PX4_OK on success, PX4_ERROR on failure.
*/
uint8_t get_pec(uint8_t *buffer, uint8_t length);
};
device::Device *
BATT_SMBUS_I2C_interface(int bus)
{
return new BATT_SMBUS_I2C(bus);
}
BATT_SMBUS_I2C::BATT_SMBUS_I2C(int bus) :
I2C("BATT_SMBUS_I2C", nullptr, bus, BATT_SMBUS_ADDR, 100000)
{
}
int
BATT_SMBUS_I2C::read(unsigned cmd_code, void *data, unsigned length)
{
uint8_t buf = (uint8_t) cmd_code;
return transfer(&buf, 1, (uint8_t *)data, length);
}
int
BATT_SMBUS_I2C::write(unsigned cmd_code, void *data, unsigned length)
{
return transfer((uint8_t *)data, length, nullptr, 0);
}

530
src/drivers/batt_smbus/batt_smbus_main.cpp Normal file
View File

@ -0,0 +1,530 @@
/****************************************************************************
*
* Copyright (c) 2018 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.
*
****************************************************************************/
#include "batt_smbus.h"
#include <px4_module.h>
extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]);
enum BATT_SMBUS_BUS {
BATT_SMBUS_BUS_ALL = 0,
BATT_SMBUS_BUS_I2C_INTERNAL,
BATT_SMBUS_BUS_I2C_EXTERNAL,
};
/**
* Local functions in support of the shell command.
*/
namespace batt_smbus
{
struct batt_smbus_bus_option {
enum BATT_SMBUS_BUS busid;
const char *devpath;
BATT_SMBUS_constructor interface_constructor;
uint8_t busnum;
BATT_SMBUS *dev;
} bus_options[] = {
{ BATT_SMBUS_BUS_I2C_EXTERNAL, "/dev/batt_smbus_ext", &BATT_SMBUS_I2C_interface, PX4_I2C_BUS_EXPANSION, nullptr },
#ifdef PX4_I2C_BUS_ONBOARD
{ BATT_SMBUS_BUS_I2C_INTERNAL, "/dev/batt_smbus_int", &BATT_SMBUS_I2C_interface, PX4_I2C_BUS_ONBOARD, nullptr },
#endif
};
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
int start(enum BATT_SMBUS_BUS busid);
bool start_bus(struct batt_smbus_bus_option &bus);
struct batt_smbus_bus_option &find_bus(enum BATT_SMBUS_BUS busid);
int reset(enum BATT_SMBUS_BUS busid);
int info();
void usage(const char *reason);
int manufacture_date();
int manufacturer_name();
int serial_number();
/**
* start driver for a specific bus option
*/
bool start_bus(struct batt_smbus_bus_option &bus)
{
PX4_INFO("starting %s", bus.devpath);
if (bus.dev != nullptr) {
PX4_WARN("bus option already started");
return false;
}
device::Device *interface = bus.interface_constructor(bus.busnum);
if (interface->init() != OK) {
delete interface;
PX4_WARN("no device on bus %u", (unsigned)bus.busid);
return false;
}
bus.dev = new BATT_SMBUS(interface, bus.devpath);
if (bus.dev != nullptr && PX4_OK != bus.dev->init()) {
PX4_WARN("init failed");
delete bus.dev;
bus.dev = nullptr;
return false;
}
return true;
}
/**
* Start the driver.
*
* This function call only returns once the driver
* is either successfully up and running or failed to start.
*/
int start(enum BATT_SMBUS_BUS busid)
{
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (busid == BATT_SMBUS_BUS_ALL && bus_options[i].dev != nullptr) {
// This device is already started.
PX4_INFO("Smart battery %d already started", bus_options[i].dev);
continue;
}
if (busid != BATT_SMBUS_BUS_ALL && bus_options[i].busid != busid) {
// Not the one that is asked for.
continue;
}
if (start_bus(bus_options[i])) {
return PX4_OK;
}
PX4_INFO("Smart battery failed to start");
return PX4_ERROR;
}
return PX4_ERROR;
}
/**
* Find a bus structure for a busid.
*/
struct batt_smbus_bus_option &find_bus(enum BATT_SMBUS_BUS busid)
{
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if ((busid == BATT_SMBUS_BUS_ALL || busid == bus_options[i].busid) &&
bus_options[i].dev != nullptr) {
return bus_options[i];
}
}
errx(1, "Could not find a smart battery: Did you start it?");
// to satisfy other compilers
return bus_options[0];
}
int manufacture_date()
{
int result = -1;
uint16_t man_date = 0;
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
struct batt_smbus_bus_option &bus = bus_options[i];
if (bus.dev != nullptr) {
PX4_INFO("%s", bus.devpath);
result = bus.dev->manufacture_date(&man_date);
}
}
if (PX4_OK == result) {
// Convert the uint16_t into human-readable date format.
uint16_t year = ((man_date >> 9) & 0xFF) + 1980;
uint8_t month = (man_date >> 5) & 0xF;
uint8_t day = man_date & 0x1F;
PX4_INFO("The manufacturer date is: %4d-%02d-%02d", year, month, day);
return PX4_OK;
} else {
PX4_WARN("Unable to read the manufacturer date.");
return PX4_ERROR;
}
}
int manufacturer_name()
{
int result = -1;
uint8_t man_name[22];
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
struct batt_smbus_bus_option &bus = bus_options[i];
if (bus.dev != nullptr) {
PX4_INFO("%s", bus.devpath);
result = bus.dev->manufacturer_name(man_name, sizeof(man_name));
}
}
if (PX4_OK == result) {
PX4_INFO("The manufacturer name: %s", man_name);
return PX4_OK;
} else {
PX4_WARN("Unable to read manufacturer name.");
return PX4_ERROR;
}
}
int serial_number()
{
uint16_t serial_num = 0;
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
struct batt_smbus_bus_option &bus = bus_options[i];
if (bus.dev != nullptr) {
PX4_INFO("%s", bus.devpath);
serial_num = bus.dev->get_serial_number();
}
}
PX4_INFO("The serial number: 0x%04x (%d in decimal)", serial_num, serial_num);
return PX4_OK;
}
void usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
GPS driver module that handles the communication with the device and publishes the position via uORB.
It supports multiple protocols (device vendors) and by default automatically selects the correct one.
The module supports a secondary GPS device, specified via `-e` parameter. The position will be published
on the second uORB topic instance, but it's currently not used by the rest of the system (however the
data will be logged, so that it can be used for comparisons).
### Implementation
There is a thread for each device polling for data. The GPS protocol classes are implemented with callbacks
so that they can be used in other projects as well (eg. QGroundControl uses them too).
### Examples
For testing it can be useful to fake a GPS signal (it will signal the system that it has a valid position):
$ gps stop
$ gps start -f
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("batt_smbus", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAM_STRING('X', "BATT_SMBUS_BUS_I2C_EXTERNAL", nullptr, nullptr, true);
PRINT_MODULE_USAGE_PARAM_STRING('I', "BATT_SMBUS_BUS_I2C_INTERNAL", nullptr, nullptr, true);
PRINT_MODULE_USAGE_PARAM_STRING('A', "BATT_SMBUS_BUS_ALL", nullptr, nullptr, true);
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stops the driver.");
PRINT_MODULE_USAGE_COMMAND_DESCR("suspend", "Suspends the drive but does stop it completely.");
PRINT_MODULE_USAGE_COMMAND_DESCR("resume", "Resumes the driver from the suspended state.");
PRINT_MODULE_USAGE_COMMAND_DESCR("man_nam", "Prints the name of the manufacturer.");
PRINT_MODULE_USAGE_COMMAND_DESCR("man_date", "Prints the date of manufacture.");
PRINT_MODULE_USAGE_COMMAND_DESCR("serial_num", "Prints the serial number.");
PRINT_MODULE_USAGE_COMMAND_DESCR("sbs_info", "Prints the manufacturer name, date, and serial number.");
PRINT_MODULE_USAGE_COMMAND_DESCR("info", "Prints the last report.");
PRINT_MODULE_USAGE_COMMAND_DESCR("unseal", "Unseals the devices flash memory to enable write_flash commands.");
PRINT_MODULE_USAGE_COMMAND_DESCR("read_word", "Uses the SMbus read-word command.");
PRINT_MODULE_USAGE_ARG("command code", "The SMbus command .", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("man_read", "Uses the SMbus block-read with ManufacturerAccess().");
PRINT_MODULE_USAGE_ARG("command code", "The SMbus command .", true);
PRINT_MODULE_USAGE_ARG("number of bytes", "Number of bytes to read.", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("read_flash", "Reads 32 bytes from flash starting from the address specified.");
PRINT_MODULE_USAGE_ARG("address", "The address to start reading from. .", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("write_flash", "Writes to flash. The device must first be unsealed with the unseal command.");
PRINT_MODULE_USAGE_ARG("address", "The address to start writing.", true);
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_COMMAND_DESCR("block_read", "Performs a SMBus block read.");
PRINT_MODULE_USAGE_ARG("command code", "The SMbus command .", true);
PRINT_MODULE_USAGE_ARG("number of bytes", "Number of bytes to read.", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("block_write", "Performs a SMBus block write.");
PRINT_MODULE_USAGE_ARG("command code", "The SMbus command code.", true);
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);
}
} //namespace
int
batt_smbus_main(int argc, char *argv[])
{
enum BATT_SMBUS_BUS busid = BATT_SMBUS_BUS_I2C_EXTERNAL;
int ch;
// Jump over start/off/etc and look at options first.
while ((ch = getopt(argc, argv, "XIA:")) != EOF) {
switch (ch) {
case 'X':
busid = BATT_SMBUS_BUS_I2C_EXTERNAL;
break;
case 'I':
busid = BATT_SMBUS_BUS_I2C_INTERNAL;
break;
case 'A':
busid = BATT_SMBUS_BUS_ALL;
break;
default:
batt_smbus::usage("unrecognized argument");
return 0;
}
}
const char *input = argv[optind];
if(!input) {
batt_smbus::usage("Please enter an appropriate command.");
return 1;
}
if (!strcmp(input, "start")) {
return batt_smbus::start(busid);
}
struct batt_smbus::batt_smbus_bus_option &bus = batt_smbus::find_bus(busid);
if (!strcmp(input, "stop")) {
delete bus.dev;
bus.dev = nullptr;
return 0;
}
if (!strcmp(input, "suspend")) {
bus.dev->stop();
return 0;
}
if (!strcmp(input, "resume")) {
bus.dev->start();
return 0;
}
if (!strcmp(input, "man_name")) {
batt_smbus::manufacturer_name();
return 0;
}
if (!strcmp(input, "man_date")) {
batt_smbus::manufacture_date();
return 0;
}
if (!strcmp(input, "serial_num")) {
batt_smbus::serial_number();
return 0;
}
if (!strcmp(input, "sbs_info")) {
batt_smbus::manufacturer_name();
batt_smbus::manufacture_date();
batt_smbus::serial_number();
return 0;
}
if (!strcmp(input, "info")) {
bus.dev->info();
return 0;
}
if (!strcmp(input, "unseal")) {
bus.dev->unseal();
return 0;
}
if (!strcmp(argv[2], "read_word")) {
if (argv[3]) {
uint8_t data[2];
if (PX4_OK != bus.dev->read_word((uint8_t)atoi(argv[3]), data)) {
PX4_INFO("Register read failed");
return 1;
} else {
PX4_INFO("Register value: %d %d", data[1], data[0]);
return 0;
}
}
return 1;
}
// cmd code (u16), length (32bytes max)
if (!strcmp(argv[2], "man_read")) {
if (argv[3] && argv[4]) {
uint8_t data[32] = {0};
uint16_t cmd_code = atoi(argv[3]);
unsigned length = atoi(argv[4]);
if (length > 32) {
PX4_WARN("Data length out of range: Max 32 bytes");
return 1;
}
if (PX4_OK != bus.dev->manufacturer_read(cmd_code, data, length)) {
PX4_INFO("Block write failed");
return 1;
} else {
return 0;
}
}
return 1;
}
// cmd code (u16), length (32bytes max)
if (!strcmp(argv[2], "block_read")) {
if (argv[3] && argv[4]) {
uint8_t data[32] = {0};
uint8_t cmd_code = atoi(argv[3]);
unsigned length = atoi(argv[4]);
if (length > 32) {
PX4_WARN("Data length out of range: Max 32 bytes");
return 1;
}
if (PX4_OK != bus.dev->block_read(cmd_code, data, length)) {
PX4_INFO("Block read failed");
return 1;
} else {
for (unsigned i = 0; i < length; i++) {
PX4_INFO("%d", data[i]);
}
return 0;
}
}
return 1;
}
// address, length, data1, data2, data3, etc
if (!strcmp(argv[2], "block_write")) {
if (argv[3] && argv[4]) {
uint8_t cmd_code = atoi(argv[3]);
unsigned length = atoi(argv[4]);
uint8_t tx_buf[32] = {0};
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++) {
tx_buf[i] = atoi(argv[5 + i]);
PX4_INFO("Read in: %d", tx_buf[i]);
}
if (PX4_OK != bus.dev->block_write(cmd_code, tx_buf, length)) {
PX4_INFO("Dataflash read failed");
return 1;
} else {
return 0;
}
}
return 1;
}
if (!strcmp(argv[2], "read_flash")) {
if (argv[3]) {
uint16_t address = atoi(argv[3]);
uint8_t rx_buf[32] = {0};
if (PX4_OK != bus.dev->dataflash_read(address, rx_buf)) {
PX4_INFO("Dataflash read failed");
return 1;
} else {
return 0;
}
}
return 1;
}
if (!strcmp(argv[2], "write_flash")) {
if (argv[3] && argv[4]) {
uint16_t address = atoi(argv[3]);
unsigned length = atoi(argv[4]);
uint8_t tx_buf[32] = {0};
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++) {
tx_buf[i] = atoi(argv[5 + i]);
}
if (PX4_OK != bus.dev->dataflash_write(address, tx_buf, length)) {
PX4_INFO("Dataflash write failed: %d", address);
usleep(100000);
return 1;
} else {
usleep(100000);
return 0;
}
}
}
batt_smbus::usage("unrecognized argument");
return 1;
}