AP_BattMonitor: UAVCAN battery information added
This commit is contained in:
parent
15b8ae99fe
commit
fcccc0174b
@ -2,6 +2,9 @@
|
|||||||
#include "AP_BattMonitor_Analog.h"
|
#include "AP_BattMonitor_Analog.h"
|
||||||
#include "AP_BattMonitor_SMBus.h"
|
#include "AP_BattMonitor_SMBus.h"
|
||||||
#include "AP_BattMonitor_Bebop.h"
|
#include "AP_BattMonitor_Bebop.h"
|
||||||
|
#if HAL_WITH_UAVCAN
|
||||||
|
#include "AP_BattMonitor_UAVCAN.h"
|
||||||
|
#endif
|
||||||
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||||||
#include <DataFlash/DataFlash.h>
|
#include <DataFlash/DataFlash.h>
|
||||||
|
|
||||||
@ -20,7 +23,6 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
|
|||||||
// @Path: AP_BattMonitor_Params.cpp
|
// @Path: AP_BattMonitor_Params.cpp
|
||||||
AP_SUBGROUPINFO(_params[1], "2_", 24, AP_BattMonitor, AP_BattMonitor_Params),
|
AP_SUBGROUPINFO(_params[1], "2_", 24, AP_BattMonitor, AP_BattMonitor_Params),
|
||||||
|
|
||||||
|
|
||||||
AP_GROUPEND
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
@ -83,6 +85,12 @@ AP_BattMonitor::init()
|
|||||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
|
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
|
||||||
drivers[instance] = new AP_BattMonitor_Bebop(*this, state[instance], _params[instance]);
|
drivers[instance] = new AP_BattMonitor_Bebop(*this, state[instance], _params[instance]);
|
||||||
_num_instances++;
|
_num_instances++;
|
||||||
|
#endif
|
||||||
|
break;
|
||||||
|
case AP_BattMonitor_Params::BattMonitor_TYPE_UAVCAN_BatteryInfo:
|
||||||
|
#if HAL_WITH_UAVCAN
|
||||||
|
drivers[instance] = new AP_BattMonitor_UAVCAN(*this, state[instance], AP_BattMonitor_UAVCAN::UAVCAN_BATTERY_INFO, _params[instance]);
|
||||||
|
_num_instances++;
|
||||||
#endif
|
#endif
|
||||||
break;
|
break;
|
||||||
case AP_BattMonitor_Params::BattMonitor_TYPE_NONE:
|
case AP_BattMonitor_Params::BattMonitor_TYPE_NONE:
|
||||||
|
@ -25,6 +25,7 @@ class AP_BattMonitor_Analog;
|
|||||||
class AP_BattMonitor_SMBus;
|
class AP_BattMonitor_SMBus;
|
||||||
class AP_BattMonitor_SMBus_Solo;
|
class AP_BattMonitor_SMBus_Solo;
|
||||||
class AP_BattMonitor_SMBus_Maxell;
|
class AP_BattMonitor_SMBus_Maxell;
|
||||||
|
class AP_BattMonitor_UAVCAN;
|
||||||
|
|
||||||
class AP_BattMonitor
|
class AP_BattMonitor
|
||||||
{
|
{
|
||||||
@ -33,6 +34,7 @@ class AP_BattMonitor
|
|||||||
friend class AP_BattMonitor_SMBus;
|
friend class AP_BattMonitor_SMBus;
|
||||||
friend class AP_BattMonitor_SMBus_Solo;
|
friend class AP_BattMonitor_SMBus_Solo;
|
||||||
friend class AP_BattMonitor_SMBus_Maxell;
|
friend class AP_BattMonitor_SMBus_Maxell;
|
||||||
|
friend class AP_BattMonitor_UAVCAN;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
AP_BattMonitor(uint32_t log_battery_bit);
|
AP_BattMonitor(uint32_t log_battery_bit);
|
||||||
|
@ -49,6 +49,10 @@ public:
|
|||||||
// update battery resistance estimate and voltage_resting_estimate
|
// update battery resistance estimate and voltage_resting_estimate
|
||||||
void update_resistance_estimate();
|
void update_resistance_estimate();
|
||||||
|
|
||||||
|
// callback for UAVCAN messages
|
||||||
|
virtual void handle_bi_msg(float voltage, float current,
|
||||||
|
float temperature) {}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
AP_BattMonitor &_mon; // reference to front-end
|
AP_BattMonitor &_mon; // reference to front-end
|
||||||
AP_BattMonitor::BattMonitor_State &_state; // reference to this instances state (held in the front-end)
|
AP_BattMonitor::BattMonitor_State &_state; // reference to this instances state (held in the front-end)
|
||||||
|
@ -7,7 +7,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
|
|||||||
// @Param: MONITOR
|
// @Param: MONITOR
|
||||||
// @DisplayName: Battery monitoring
|
// @DisplayName: Battery monitoring
|
||||||
// @Description: Controls enabling monitoring of the battery's voltage and current
|
// @Description: Controls enabling monitoring of the battery's voltage and current
|
||||||
// @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell
|
// @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell,8:UAVCAN-BatteryInfo
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, BattMonitor_TYPE_NONE, AP_PARAM_FLAG_ENABLE),
|
AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, BattMonitor_TYPE_NONE, AP_PARAM_FLAG_ENABLE),
|
||||||
|
|
||||||
@ -63,7 +63,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
|
|||||||
|
|
||||||
// @Param: SERIAL_NUM
|
// @Param: SERIAL_NUM
|
||||||
// @DisplayName: Battery serial number
|
// @DisplayName: Battery serial number
|
||||||
// @Description: Battery serial number, automatically filled in for SMBus batteries, otherwise will be -1
|
// @Description: Battery serial number, automatically filled in for SMBus batteries, otherwise will be -1. With UAVCAN it is the battery_id.
|
||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
AP_GROUPINFO("SERIAL_NUM", 9, AP_BattMonitor_Params, _serial_number, AP_BATT_SERIAL_NUMBER_DEFAULT),
|
AP_GROUPINFO("SERIAL_NUM", 9, AP_BattMonitor_Params, _serial_number, AP_BATT_SERIAL_NUMBER_DEFAULT),
|
||||||
|
|
||||||
|
@ -19,7 +19,8 @@ public:
|
|||||||
BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT = 4,
|
BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT = 4,
|
||||||
BattMonitor_TYPE_SOLO = 5,
|
BattMonitor_TYPE_SOLO = 5,
|
||||||
BattMonitor_TYPE_BEBOP = 6,
|
BattMonitor_TYPE_BEBOP = 6,
|
||||||
BattMonitor_TYPE_MAXELL = 7
|
BattMonitor_TYPE_MAXELL = 7,
|
||||||
|
BattMonitor_TYPE_UAVCAN_BatteryInfo = 8
|
||||||
};
|
};
|
||||||
|
|
||||||
// low voltage sources (used for BATT_LOW_TYPE parameter)
|
// low voltage sources (used for BATT_LOW_TYPE parameter)
|
||||||
|
80
libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp
Normal file
80
libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp
Normal file
@ -0,0 +1,80 @@
|
|||||||
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
|
#if HAL_WITH_UAVCAN
|
||||||
|
|
||||||
|
#include <AP_Common/AP_Common.h>
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
|
#include "AP_BattMonitor.h"
|
||||||
|
#include "AP_BattMonitor_UAVCAN.h"
|
||||||
|
|
||||||
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||||
|
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
|
||||||
|
|
||||||
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
|
#define debug_bm_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig_CAN::get_can_debug()) { printf(fmt, ##args); }} while (0)
|
||||||
|
|
||||||
|
/// Constructor
|
||||||
|
AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params ¶ms) :
|
||||||
|
AP_BattMonitor_Backend(mon, mon_state, params),
|
||||||
|
_type(type)
|
||||||
|
{
|
||||||
|
// starts with not healthy
|
||||||
|
_state.healthy = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_BattMonitor_UAVCAN::init()
|
||||||
|
{
|
||||||
|
if (AP_BoardConfig_CAN::get_can_num_ifaces() != 0) {
|
||||||
|
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
|
||||||
|
if (hal.can_mgr[i] != nullptr) {
|
||||||
|
AP_UAVCAN *uavcan = hal.can_mgr[i]->get_UAVCAN();
|
||||||
|
if (uavcan != nullptr) {
|
||||||
|
switch (_type) {
|
||||||
|
case UAVCAN_BATTERY_INFO:
|
||||||
|
if (uavcan->register_BM_bi_listener_to_id(this, _params._serial_number)) {
|
||||||
|
debug_bm_uavcan(2, "UAVCAN BattMonitor BatteryInfo registered id: %d\n\r", _params._serial_number);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// read - read the voltage and current
|
||||||
|
void AP_BattMonitor_UAVCAN::read()
|
||||||
|
{
|
||||||
|
uint32_t tnow = AP_HAL::micros();
|
||||||
|
|
||||||
|
// timeout after 5 seconds
|
||||||
|
if ((tnow - _state.last_time_micros) > AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS) {
|
||||||
|
_state.healthy = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_BattMonitor_UAVCAN::handle_bi_msg(float voltage, float current, float temperature)
|
||||||
|
{
|
||||||
|
_state.temperature = temperature;
|
||||||
|
_state.voltage = voltage;
|
||||||
|
_state.current_amps = current;
|
||||||
|
|
||||||
|
uint32_t tnow = AP_HAL::micros();
|
||||||
|
uint32_t dt = tnow - _state.last_time_micros;
|
||||||
|
|
||||||
|
// update total current drawn since startup
|
||||||
|
if (_state.last_time_micros != 0 && dt < 2000000) {
|
||||||
|
// .0002778 is 1/3600 (conversion to hours)
|
||||||
|
float mah = (float) ((double) _state.current_amps * (double) dt * (double) 0.0000002778f);
|
||||||
|
_state.current_total_mah += mah;
|
||||||
|
_state.consumed_wh += 0.001f * mah * _state.voltage;
|
||||||
|
}
|
||||||
|
|
||||||
|
// record time
|
||||||
|
_state.last_time_micros = tnow;
|
||||||
|
|
||||||
|
_state.healthy = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
33
libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h
Normal file
33
libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h
Normal file
@ -0,0 +1,33 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <AP_UAVCAN/AP_UAVCAN.h>
|
||||||
|
#include "AP_BattMonitor.h"
|
||||||
|
#include "AP_BattMonitor_Backend.h"
|
||||||
|
|
||||||
|
#define AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS 5000000 // sensor becomes unhealthy if no successful readings for 5 seconds
|
||||||
|
|
||||||
|
class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
|
||||||
|
enum BattMonitor_UAVCAN_Type {
|
||||||
|
UAVCAN_BATTERY_INFO = 0
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Constructor
|
||||||
|
AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params ¶ms);
|
||||||
|
|
||||||
|
/// Read the battery voltage and current. Should be called at 10hz
|
||||||
|
void read() override;
|
||||||
|
|
||||||
|
void init() override;
|
||||||
|
|
||||||
|
bool has_current() const override {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void handle_bi_msg(float voltage, float current, float temperature) override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
BattMonitor_UAVCAN_Type _type;
|
||||||
|
};
|
Loading…
Reference in New Issue
Block a user