fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-06 15:11:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated batt_smbus. Expanded battery_status.msg. Fixed mavlink_messages.cpp temperature. (#8991)

Browse Source 
* Updated and expanded batt_smbus to work with bq40z50-R2. Expanded battery_status.msg. Fixed mavlink_messages.cpp temperature, added commented out expanded battery_status.msg parameters for future mavlink expansion.

* Changed errx to PX4_ERR

* Added PX4_ERR returns
This commit is contained in:
AlexKlimaj
2018-03-06 10:42:20 +01:00
committed by Lorenz Meier
parent bf097d7fa4
commit 83d01a7c76
3 changed files with 180 additions and 273 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/battery_status.msg
+8
View File
@@ -2,13 +2,21 @@ float32 voltage_v # Battery voltage in volts, 0 if unknown
float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown
float32 current_a # Battery current in amperes, -1 if unknown float32 current_a # Battery current in amperes, -1 if unknown
float32 current_filtered_a # Battery current in amperes, filtered, 0 if unknown float32 current_filtered_a # Battery current in amperes, filtered, 0 if unknown
float32 average_current_a # Battery current average in amperes, -1 if unknown
float32 discharged_mah # Discharged amount in mAh, -1 if unknown float32 discharged_mah # Discharged amount in mAh, -1 if unknown
float32 remaining # From 1 to 0, -1 if unknown float32 remaining # From 1 to 0, -1 if unknown
float32 scale # Power scaling factor, >= 1, or -1 if unknown float32 scale # Power scaling factor, >= 1, or -1 if unknown
float32 temperature # temperature of the battery
int32 cell_count # Number of cells int32 cell_count # Number of cells
bool connected # Whether or not a battery is connected, based on a voltage threshold bool connected # Whether or not a battery is connected, based on a voltage threshold
bool system_source # Whether or not a this battery is the active power source for VDD_5V_IN bool system_source # Whether or not a this battery is the active power source for VDD_5V_IN
uint8 priority # Zerro based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 uint8 priority # Zerro based priority is the connection on the Power Controller V1..Vn AKA BrickN-1
uint16 capacity # actual capacity of the battery
uint16 cycle_count # number of discharge cycles the battery has experienced
uint16 run_time_to_empty # predicted remaining battery capacity based on the present rate of discharge in min
uint16 average_time_to_empty # predicted remaining battery capacity based on the average rate of discharge in min
uint16 serial_number # serial number of the battery pack
bool is_powering_off # Power off event imminent indication, false if unknown bool is_powering_off # Power off event imminent indication, false if unknown
src/drivers/batt_smbus/batt_smbus.cpp
+165 -272
View File
@@ -35,8 +35,10 @@
* @file batt_smbus.cpp * @file batt_smbus.cpp
* *
* Driver for a battery monitor connected via SMBus (I2C). * Driver for a battery monitor connected via SMBus (I2C).
* Designed for BQ40Z50-R1/R2
* *
* @author Randy Mackay <rmackay9@yahoo.com> * @author Randy Mackay <rmackay9@yahoo.com>
* @author Alex Klimaj <alexklimaj@gmail.com>
*/ */
#include <float.h> #include <float.h>
@@ -53,47 +55,37 @@
#include <uORB/topics/subsystem_info.h> #include <uORB/topics/subsystem_info.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#define BATT_SMBUS_ADDR_MIN 0x08 ///< lowest possible address #define BATT_SMBUS_ADDR_MIN 0x00 ///< lowest possible address
#define BATT_SMBUS_ADDR_MAX 0x7F ///< highest possible address #define BATT_SMBUS_ADDR_MAX 0xFF ///< highest possible address
#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION #define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION
#define BATT_SMBUS_ADDR 0x0B ///< I2C address #define BATT_SMBUS_ADDR 0x0B ///< Default 7 bit address I2C address. 8 bit = 0x16
#define BATT_SMBUS_TEMP 0x08 ///< temperature register #define BATT_SMBUS_TEMP 0x08 ///< temperature register
#define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register #define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register
#define BATT_SMBUS_REMAINING_CAPACITY 0x0f ///< predicted remaining battery capacity as a percentage #define BATT_SMBUS_REMAINING_CAPACITY 0x0F ///< predicted remaining battery capacity as a percentage
#define BATT_SMBUS_FULL_CHARGE_CAPACITY 0x10 ///< capacity when fully charged #define BATT_SMBUS_FULL_CHARGE_CAPACITY 0x10 ///< capacity when fully charged
#define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register #define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register
#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register
#define BATT_SMBUS_MANUFACTURE_DATE 0x1B ///< manufacture date register #define BATT_SMBUS_MANUFACTURE_DATE 0x1B ///< manufacture date register
#define BATT_SMBUS_SERIAL_NUMBER 0x1C ///< serial number register #define BATT_SMBUS_SERIAL_NUMBER 0x1C ///< serial number register
#define BATT_SMBUS_MANUFACTURER_NAME 0x20 ///< manufacturer name #define BATT_SMBUS_MANUFACTURER_NAME 0x20 ///< manufacturer name
#define BATT_SMBUS_DEVICE_NAME 0x21 ///< device name register #define BATT_SMBUS_CURRENT 0x0A ///< current register
#define BATT_SMBUS_DEVICE_CHEMISTRY 0x22 ///< device chemistry register #define BATT_SMBUS_AVERAGE_CURRENT 0x0B ///< current register
#define BATT_SMBUS_MANUFACTURER_DATA 0x23 ///< manufacturer data
#define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register
#define BATT_SMBUS_CURRENT 0x2a ///< current register
#define BATT_SMBUS_MEASUREMENT_INTERVAL_US (1000000 / 10) ///< time in microseconds, measure at 10Hz #define BATT_SMBUS_MEASUREMENT_INTERVAL_US (1000000 / 10) ///< time in microseconds, measure at 10Hz
#define BATT_SMBUS_TIMEOUT_US 10000000 ///< timeout looking for battery 10seconds after startup #define BATT_SMBUS_TIMEOUT_US 10000000 ///< timeout looking for battery 10seconds after startup
#define BATT_SMBUS_CYCLE_COUNT 0x17 ///< number of cycles the battery has experienced
#define BATT_SMBUS_BUTTON_DEBOUNCE_MS 300 ///< button holds longer than this time will cause a power off event #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_MANUFACTURER_ACCESS 0x00 #define BATT_SMBUS_MANUFACTURER_ACCESS 0x00
#define BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS 0x44 #define BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS 0x44
#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC #define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC
#ifndef CONFIG_SCHED_WORKQUEUE #ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE. # error This requires CONFIG_SCHED_WORKQUEUE.
#endif #endif
struct battery_type {
char *ManufacturerName;
char *DeviceName;
char *DeviceChemistry;
};
// Declaration of the solo battery data, as determined by reading out the data from multiple 3DR Solo batteries
const struct battery_type solo_battery = {(char *)"BMTPOW", (char *)"MA03", (char *)"LIon"};
class BATT_SMBUS : public device::I2C class BATT_SMBUS : public device::I2C
{ {
@@ -148,36 +140,11 @@ public:
*/ */
uint16_t manufacture_date(); uint16_t manufacture_date();
/**
* Get the SBS device name of the battery device
*
* @param dev_name pointer a buffer into which the device name is to be written
* @param max_length the maximum number of bytes to attempt to read from the device name register, including the null character that is appended to the end
*
* @return the number of bytes read
*/
uint8_t device_name(uint8_t *dev_name, uint8_t max_length);
/** /**
* Return the SBS serial number of the battery device * Return the SBS serial number of the battery device
*/ */
uint16_t serial_number(); uint16_t serial_number();
/**
* Get the SBS device chemistry of the battery device
*
* @param dev_chem pointer a buffer into which the device chemistry is to be written
* @param max_length the maximum number of bytes to attempt to read from the device chemistry register, including the null character that is appended to the end
*
* @return the number of bytes read
*/
uint8_t device_chemistry(uint8_t *dev_chem, uint8_t max_length);
/**
* Checks whether the current SBS battery data corresponds to a 3DR Solo battery
*/
bool is_solo_battery();
protected: protected:
/** /**
* Check if the device can be contacted * Check if the device can be contacted
@@ -216,6 +183,12 @@ private:
*/ */
int write_reg(uint8_t reg, uint16_t val); int write_reg(uint8_t reg, uint16_t val);
/**
* Convert from 2's compliment to decimal
* @return the absolute value of the input in decimal
*/
uint16_t convert_twos_comp(uint16_t val);
/** /**
* Read block from bus * Read block from bus
* @return returns number of characters read if successful, zero if unsuccessful * @return returns number of characters read if successful, zero if unsuccessful
@@ -232,19 +205,13 @@ private:
* Calculate PEC for a read or write from the battery * Calculate PEC for a read or write from the battery
* @param buff is the data that was read or will be written * @param buff is the data that was read or will be written
*/ */
uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len); uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len);
/** /**
* Write a word to Manufacturer Access register (0x00) * Write a word to Manufacturer Access register (0x00)
* @param cmd the word to be written to Manufacturer Access * @param cmd the word to be written to Manufacturer Access
*/ */
uint8_t ManufacturerAccess(uint16_t cmd); uint8_t ManufacturerAccess(uint16_t cmd);
/**
* Checks if the battery that has been detected is a 3DR Solo Battery. If it is, it sets
* the private variable _is_solo_battery to be true
*/
void check_if_solo_battery();
// internal variables // internal variables
bool _enabled; ///< true if we have successfully connected to battery bool _enabled; ///< true if we have successfully connected to battery
@@ -252,16 +219,18 @@ private:
battery_status_s _last_report; ///< last published report, used for test() battery_status_s _last_report; ///< last published report, used for test()
orb_advert_t _batt_topic; ///< uORB battery topic orb_advert_t _batt_topic; ///< uORB battery topic
orb_id_t _batt_orb_id; ///< uORB battery topic ID orb_id_t _batt_orb_id; ///< uORB battery topic ID
uint64_t _start_time; ///< system time we first attempt to communicate with battery uint64_t _start_time; ///< system time we first attempt to communicate with battery
uint16_t _batt_capacity; ///< battery's design capacity in mAh (0 means unknown) uint16_t _batt_capacity; ///< battery's design capacity in mAh (0 means unknown)
uint16_t _batt_startup_capacity; ///< battery's remaining capacity on startup
char *_manufacturer_name; ///< The name of the battery manufacturer char *_manufacturer_name; ///< The name of the battery manufacturer
char *_device_name; ///< The name of the battery device uint16_t _cycle_count; ///< number of cycles the battery has experienced
char *_device_chemistry; ///< The battery chemistry uint16_t _serial_number; ///< serial number register
bool _is_solo_battery; ///< Boolean as to whether the battery detected is a 3DR Solo Battery or not float _crit_thr; ///< Critical battery threshold param
uint8_t _button_press_counts; ///< count of button presses detected on 3DR Solo Battery float _low_thr; ///< Low battery threshold param
float _emergency_thr; ///< Emergency battery threshold param
}; };
namespace namespace
@@ -275,10 +244,7 @@ extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]);
int manufacturer_name(); int manufacturer_name();
int manufacture_date(); int manufacture_date();
int device_name();
int serial_number(); int serial_number();
int device_chemistry();
int solo_battery_check();
BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) :
I2C("batt_smbus", "/dev/batt_smbus0", bus, batt_smbus_addr, 100000), I2C("batt_smbus", "/dev/batt_smbus0", bus, batt_smbus_addr, 100000),
@@ -287,11 +253,13 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) :
_batt_orb_id(nullptr), _batt_orb_id(nullptr),
_start_time(0), _start_time(0),
_batt_capacity(0), _batt_capacity(0),
_batt_startup_capacity(0),
_manufacturer_name(nullptr), _manufacturer_name(nullptr),
_device_name(nullptr), _cycle_count(0),
_device_chemistry(nullptr), _serial_number(0),
_is_solo_battery(false), _crit_thr(0.0f),
_button_press_counts(0) _low_thr(0.0f),
_emergency_thr(0.0f)
{ {
// capture startup time // capture startup time
_start_time = hrt_absolute_time(); _start_time = hrt_absolute_time();
@@ -305,14 +273,6 @@ BATT_SMBUS::~BATT_SMBUS()
if (_manufacturer_name != nullptr) { if (_manufacturer_name != nullptr) {
delete[] _manufacturer_name; delete[] _manufacturer_name;
} }
if (_device_name != nullptr) {
delete[] _device_name;
}
if (_device_chemistry != nullptr) {
delete[] _device_chemistry;
}
} }
int int
@@ -328,6 +288,9 @@ BATT_SMBUS::init()
return ret; return ret;
} else { } else {
//Find the battery on the bus
search();
// start work queue // start work queue
start(); start();
} }
@@ -346,21 +309,23 @@ BATT_SMBUS::test()
struct battery_status_s status; struct battery_status_s status;
uint64_t start_time = hrt_absolute_time(); uint64_t start_time = hrt_absolute_time();
// loop for 5 seconds // loop for 3 seconds
while ((hrt_absolute_time() - start_time) < 5000000) { while ((hrt_absolute_time() - start_time) < 3000000) {
// display new info that has arrived from the orb // display new info that has arrived from the orb
orb_check(sub, &updated); orb_check(sub, &updated);
if (updated) { if (updated) {
if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) {
PX4_INFO("V=%4.2f C=%4.2f DismAh=%4.2f Cap:%d Shutdown:%d", (double)status.voltage_v, (double)status.current_a, PX4_INFO("V=%4.2f C=%4.2f AveC=%4.2f DismAh=%f Cap:%hu TempC:%4.2f Remaining:%3.2f\n RunTimeToEmpty:%hu AveTimeToEmpty:%hu CycleCount:%hu SerialNum:%04x",
(double)status.discharged_mah, (int)_batt_capacity, (int)status.is_powering_off); (double)status.voltage_v, (double)status.current_a, (double)status.average_current_a, (double)status.discharged_mah,
(uint16_t)status.capacity, (double)status.temperature, (double)status.remaining, (uint16_t)status.run_time_to_empty,
(uint16_t)status.average_time_to_empty, (uint16_t)status.cycle_count, (uint16_t)status.serial_number);
} }
} }
// sleep for 0.05 seconds // sleep for 0.2 seconds
usleep(50000); usleep(200000);
} }
return OK; return OK;
@@ -374,27 +339,32 @@ BATT_SMBUS::search()
int16_t orig_addr = get_device_address(); int16_t orig_addr = get_device_address();
// search through all valid SMBus addresses // search through all valid SMBus addresses
for (uint8_t i = BATT_SMBUS_ADDR_MIN; i <= BATT_SMBUS_ADDR_MAX; i++) { for (uint8_t i = BATT_SMBUS_ADDR_MIN; i < BATT_SMBUS_ADDR_MAX; i++) {
set_device_address(i); set_device_address(i);
if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) {
PX4_INFO("battery found at 0x%x", (int)i); if (tmp > 0) {
found_slave = true; PX4_INFO("battery found at 0x%x", get_device_address());
found_slave = true;
break;
}
} }
// short sleep // short sleep
usleep(1); usleep(1);
} }
// restore original i2c address if (found_slave == false) {
set_device_address(orig_addr); // restore original i2c address
set_device_address(orig_addr);
}
// display completion message // display completion message
if (found_slave) { if (found_slave) {
PX4_INFO("Done."); PX4_INFO("smart battery connected");
} else { } else {
PX4_WARN("No smart batteries found."); PX4_INFO("No smart batteries found.");
} }
return OK; return OK;
@@ -430,23 +400,6 @@ BATT_SMBUS::manufacture_date()
return 0; return 0;
} }
uint8_t
BATT_SMBUS::device_name(uint8_t *dev_name, uint8_t max_length)
{
uint8_t len = read_block(BATT_SMBUS_DEVICE_NAME, dev_name, max_length, false);
if (len > 0) {
if (len >= max_length - 1) {
dev_name[max_length - 1] = 0;
} else {
dev_name[len] = 0;
}
}
return len;
}
uint16_t uint16_t
BATT_SMBUS::serial_number() BATT_SMBUS::serial_number()
{ {
@@ -459,30 +412,6 @@ BATT_SMBUS::serial_number()
return -1; return -1;
} }
uint8_t
BATT_SMBUS::device_chemistry(uint8_t *dev_chem, uint8_t max_length)
{
uint8_t len = read_block(BATT_SMBUS_DEVICE_CHEMISTRY, dev_chem, max_length, false);
if (len > 0) {
if (len >= max_length - 1) {
dev_chem[max_length - 1] = 0;
} else {
dev_chem[len] = 0;
}
}
return len;
}
bool
BATT_SMBUS::is_solo_battery()
{
check_if_solo_battery();
return _is_solo_battery;
}
int int
BATT_SMBUS::probe() BATT_SMBUS::probe()
{ {
@@ -519,12 +448,10 @@ BATT_SMBUS::cycle()
// exit without rescheduling if we have failed to find a battery after 10 seconds // exit without rescheduling if we have failed to find a battery after 10 seconds
if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_US)) { if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_US)) {
PX4_WARN("did not find smart battery"); PX4_INFO("did not find smart battery");
return; return;
} }
bool perform_solo_battry_check = false; // Only check if it is a solo battery if changes have been made to the SBS data
// Try and get battery SBS info // Try and get battery SBS info
if (_manufacturer_name == nullptr) { if (_manufacturer_name == nullptr) {
char man_name[21]; char man_name[21];
@@ -533,36 +460,49 @@ BATT_SMBUS::cycle()
if (len > 0) { if (len > 0) {
_manufacturer_name = new char[len + 1]; _manufacturer_name = new char[len + 1];
strcpy(_manufacturer_name, man_name); strcpy(_manufacturer_name, man_name);
perform_solo_battry_check = true;
} }
} }
if (_device_name == nullptr) { // read battery serial number on startup
char dev_name[21]; if (_serial_number == 0) {
uint8_t len = device_name((uint8_t *)dev_name, sizeof(dev_name)); _serial_number = serial_number();
}
if (len > 0) { // temporary variable for storing SMBUS reads
_device_name = new char[len + 1]; uint16_t tmp;
strcpy(_device_name, dev_name);
perform_solo_battry_check = true; // read battery capacity on startup
if (_batt_startup_capacity == 0) {
if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) {
_batt_startup_capacity = tmp;
} }
} }
if (_device_chemistry == nullptr) { // read battery cycle count on startup
char dev_chem[21]; if (_cycle_count == 0) {
uint8_t len = device_chemistry((uint8_t *)dev_chem, sizeof(dev_chem)); if (read_reg(BATT_SMBUS_CYCLE_COUNT, tmp) == OK) {
_cycle_count = tmp;
if (len > 0) {
_device_chemistry = new char[len + 1];
strcpy(_device_chemistry, dev_chem);
perform_solo_battry_check = true;
} }
} }
// If necessary, check if the battery is a 3DR Solo Battery // read battery design capacity on startup
if (perform_solo_battry_check) { if (_batt_capacity == 0) {
PX4_INFO("Checking solo battery"); if (read_reg(BATT_SMBUS_FULL_CHARGE_CAPACITY, tmp) == OK) {
check_if_solo_battery(); _batt_capacity = tmp;
}
}
// read battery threshold params on startup
if (_crit_thr < 0.01f) {
param_get(param_find("BAT_CRIT_THR"), &_crit_thr);
}
if (_low_thr < 0.01f) {
param_get(param_find("BAT_LOW_THR"), &_low_thr);
}
if (_emergency_thr < 0.01f) {
param_get(param_find("BAT_EMERGEN_THR"), &_emergency_thr);
} }
// read data from sensor // read data from sensor
@@ -571,64 +511,73 @@ BATT_SMBUS::cycle()
// set time of reading // set time of reading
new_report.timestamp = now; new_report.timestamp = now;
// read voltage
uint16_t tmp;
if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) {
new_report.connected = true;
// convert millivolts to volts // convert millivolts to volts
new_report.voltage_v = ((float)tmp) / 1000.0f; new_report.voltage_v = ((float)tmp) / 1000.0f;
new_report.voltage_filtered_v = new_report.voltage_v;
// read current // read current
uint8_t buff[6]; if (read_reg(BATT_SMBUS_CURRENT, tmp) == OK) {
new_report.current_a = ((float)convert_twos_comp(tmp)) / 1000.0f;
if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { new_report.current_filtered_a = new_report.current_a;
new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 |
(uint32_t)buff[0])) / 1000.0f;
} }
// read battery design capacity // read average current
if (_batt_capacity == 0) { if (read_reg(BATT_SMBUS_AVERAGE_CURRENT, tmp) == OK) {
if (read_reg(BATT_SMBUS_FULL_CHARGE_CAPACITY, tmp) == OK) { new_report.average_current_a = ((float)convert_twos_comp(tmp)) / 1000.0f;
_batt_capacity = tmp; }
}
// read run time to empty
if (read_reg(BATT_SMBUS_RUN_TIME_TO_EMPTY, tmp) == OK) {
new_report.run_time_to_empty = tmp;
}
// read average time to empty
if (read_reg(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, tmp) == OK) {
new_report.average_time_to_empty = tmp;
} }
// read remaining capacity // read remaining capacity
if (_batt_capacity > 0) { if (_batt_capacity > 0) {
if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) { if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) {
if (tmp < _batt_capacity) {
new_report.discharged_mah = _batt_capacity - tmp; if (tmp > _batt_capacity) {
PX4_WARN("Remaining Cap greater than total: Cap:%hu RemainingCap:%hu", (uint16_t)_batt_capacity, (uint16_t)tmp);
_batt_capacity = (uint16_t)tmp;
} }
new_report.remaining = (float)(1.000f - (((float)_batt_capacity - (float)tmp) / (float)_batt_capacity));
// calculate total discharged amount
new_report.discharged_mah = (float)((float)_batt_startup_capacity - (float)tmp);
} }
} }
// if it is a solo battery, check for shutdown on button press // read battery temperature and covert to Celsius
if (_is_solo_battery) { if (read_reg(BATT_SMBUS_TEMP, tmp) == OK) {
// read the button press indicator new_report.temperature = (float)(((float)tmp / 10.0f) - 273.15f);
if (read_block(BATT_SMBUS_MANUFACTURER_DATA, buff, 6, false) == 6) {
bool pressed = (buff[1] >> 3) & 0x01;
if (_button_press_counts >= ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) {
// battery will power off
new_report.is_powering_off = true;
// warn only once
if (_button_press_counts++ == ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) {
PX4_WARN("system is shutting down NOW...");
}
} else if (pressed) {
// battery will power off if the button is held
_button_press_counts++;
} else {
// button released early, reset counters
_button_press_counts = 0;
new_report.is_powering_off = false;
}
}
} }
// propagate warning state only if the state
if (new_report.remaining < _emergency_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
} else if (new_report.remaining < _crit_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
} else if (new_report.remaining < _low_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_LOW;
} else {
new_report.warning = battery_status_s::BATTERY_WARNING_NONE;
}
new_report.capacity = _batt_capacity;
new_report.cycle_count = _cycle_count;
new_report.serial_number = _serial_number;
// publish to orb // publish to orb
if (_batt_topic != nullptr) { if (_batt_topic != nullptr) {
@@ -658,7 +607,7 @@ BATT_SMBUS::cycle()
int int
BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val)
{ {
uint8_t buff[3]; // 2 bytes of data + PEC uint8_t buff[3]; // 2 bytes of data
// read from register // read from register
int ret = transfer(&reg, 1, buff, 3); int ret = transfer(&reg, 1, buff, 3);
@@ -671,6 +620,7 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val)
val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0];
} else { } else {
PX4_ERR("BATT_SMBUS PEC Check Failed");
ret = ENOTTY; ret = ENOTTY;
} }
} }
@@ -700,6 +650,19 @@ BATT_SMBUS::write_reg(uint8_t reg, uint16_t val)
return ret; return ret;
} }
uint16_t
BATT_SMBUS::convert_twos_comp(uint16_t val)
{
if ((val & 0x8000) == 0x8000) {
uint16_t tmp;
tmp = ~val;
tmp = tmp + 1;
return tmp;
}
return val;
}
uint8_t uint8_t
BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_zero) BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_zero)
{ {
@@ -839,16 +802,6 @@ BATT_SMBUS::ManufacturerAccess(uint16_t cmd)
return ret; return ret;
} }
void
BATT_SMBUS::check_if_solo_battery()
{
// Check if the SBS information corresponds to that of a 3DR Solo Battery. If, yes, set the solo_battery flag to true;
if (!strcmp(_manufacturer_name, solo_battery.ManufacturerName) && !strcmp(_device_name, solo_battery.DeviceName)
&& !strcmp(_device_chemistry, solo_battery.DeviceChemistry)) {
_is_solo_battery = true;
}
}
///////////////////////// shell functions /////////////////////// ///////////////////////// shell functions ///////////////////////
void void
@@ -871,7 +824,7 @@ manufacturer_name()
return OK; return OK;
} else { } else {
PX4_WARN("Unable to read manufacturer name."); PX4_INFO("Unable to read manufacturer name.");
} }
return -1; return -1;
@@ -891,24 +844,7 @@ manufacture_date()
return OK; return OK;
} else { } else {
PX4_WARN("Unable to read the manufacturer date."); PX4_INFO("Unable to read the manufacturer date.");
}
return -1;
}
int
device_name()
{
uint8_t device_name[21];
uint8_t len = g_batt_smbus->device_name(device_name, sizeof(device_name));
if (len > 0) {
PX4_INFO("The device name: %s", device_name);
return OK;
} else {
PX4_WARN("Unable to read device name.");
} }
return -1; return -1;
@@ -923,36 +859,6 @@ serial_number()
return OK; return OK;
} }
int
device_chemistry()
{
uint8_t device_chemistry[5];
uint8_t len = g_batt_smbus->device_chemistry(device_chemistry, sizeof(device_chemistry));
if (len > 0) {
PX4_INFO("The device chemistry: %s", device_chemistry);
return OK;
} else {
PX4_INFO("Unable to read device chemistry.");
}
return -1;
}
int
solo_battery_check()
{
if (g_batt_smbus->is_solo_battery()) {
PX4_INFO("The battery corresponds to a 3DR Solo Battery");
} else {
PX4_INFO("The battery does not correspond to a 3DR Solo Battery");
}
return OK;
}
int int
batt_smbus_main(int argc, char *argv[]) batt_smbus_main(int argc, char *argv[])
{ {
@@ -962,7 +868,7 @@ batt_smbus_main(int argc, char *argv[])
int ch; int ch;
// jump over start/off/etc and look at options first // jump over start/off/etc and look at options first
while ((ch = getopt(argc, argv, "a:b:")) != EOF) { while ((ch = getopt(argc, argv, "a:b")) != EOF) {
switch (ch) { switch (ch) {
case 'a': case 'a':
batt_smbusadr = strtol(optarg, nullptr, 0); batt_smbusadr = strtol(optarg, nullptr, 0);
@@ -974,13 +880,13 @@ batt_smbus_main(int argc, char *argv[])
default: default:
batt_smbus_usage(); batt_smbus_usage();
exit(0); return 0;
} }
} }
if (optind >= argc) { if (optind >= argc) {
batt_smbus_usage(); batt_smbus_usage();
exit(1); return 1;
} }
const char *verb = argv[optind]; const char *verb = argv[optind];
@@ -1007,7 +913,7 @@ batt_smbus_main(int argc, char *argv[])
} }
} }
exit(0); return 0;
} }
// need the driver past this point // need the driver past this point
@@ -1043,29 +949,16 @@ batt_smbus_main(int argc, char *argv[])
return 0; return 0;
} }
if (!strcmp(verb, "dev_name")) {
device_name();
return 0;
}
if (!strcmp(verb, "serial_num")) { if (!strcmp(verb, "serial_num")) {
serial_number(); serial_number();
return 0; return 0;
} }
if (!strcmp(verb, "dev_chem")) {
device_chemistry();
return 0;
}
if (!strcmp(verb, "sbs_info")) { if (!strcmp(verb, "sbs_info")) {
manufacturer_name(); manufacturer_name();
manufacture_date(); manufacture_date();
device_name();
serial_number(); serial_number();
device_chemistry(); return 0;
solo_battery_check();
exit(0);
} }
batt_smbus_usage(); batt_smbus_usage();
src/modules/mavlink/mavlink_messages.cpp
+7 -1
View File
@@ -573,7 +573,13 @@ protected:
bat_msg.energy_consumed = -1; bat_msg.energy_consumed = -1;
bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1; bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1;
bat_msg.battery_remaining = (battery_status.connected) ? ceilf(battery_status.remaining * 100.0f) : -1; bat_msg.battery_remaining = (battery_status.connected) ? ceilf(battery_status.remaining * 100.0f) : -1;
bat_msg.temperature = INT16_MAX; bat_msg.temperature = (battery_status.connected) ? (int16_t)battery_status.temperature : INT16_MAX;
//bat_msg.average_current_battery = (battery_status.connected) ? battery_status.average_current_a * 100.0f : -1;
//bat_msg.serial_number = (battery_status.connected) ? battery_status.serial_number : 0;
//bat_msg.capacity = (battery_status.connected) ? battery_status.capacity : 0;
//bat_msg.cycle_count = (battery_status.connected) ? battery_status.cycle_count : UINT16_MAX;
//bat_msg.run_time_to_empty = (battery_status.connected) ? battery_status.run_time_to_empty * 60 : 0;
//bat_msg.average_time_to_empty = (battery_status.connected) ? battery_status.average_time_to_empty * 60 : 0;
for (unsigned int i = 0; i < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); i++) { for (unsigned int i = 0; i < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); i++) {
if ((int)i < battery_status.cell_count && battery_status.connected) { if ((int)i < battery_status.cell_count && battery_status.connected) {