zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_BattMonitor: move BattInfo message subscription and handling to AP_BattMonitor

master
Siddharth Purohit 7 years ago committed by Francisco Ferreira
parent
14b701cff8
commit
6e85003b56
4 changed files with 114 additions and 45 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      libraries/AP_BattMonitor/AP_BattMonitor.cpp
  2. 4
      libraries/AP_BattMonitor/AP_BattMonitor_Backend.h
  3. 127
      libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp
  4. 24
      libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h

4
libraries/AP_BattMonitor/AP_BattMonitor.cpp
Unescape View File

@ -3,9 +3,13 @@ @@ -3,9 +3,13 @@
#include "AP_BattMonitor_SMBus.h"
#include "AP_BattMonitor_Bebop.h"
#include "AP_BattMonitor_BLHeliESC.h"
#include <AP_HAL/AP_HAL.h>
#if HAL_WITH_UAVCAN
#include "AP_BattMonitor_UAVCAN.h"
#endif
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <DataFlash/DataFlash.h>
#include <GCS_MAVLink/GCS.h>

4
libraries/AP_BattMonitor/AP_BattMonitor_Backend.h
Unescape View File

@ -49,10 +49,6 @@ public: @@ -49,10 +49,6 @@ public:
// update battery resistance estimate and voltage_resting_estimate
void update_resistance_estimate();
// callback for UAVCAN messages
virtual void handle_bi_msg(float voltage, float current,
float temperature) {}
protected:
AP_BattMonitor &_mon; // reference to front-end
AP_BattMonitor::BattMonitor_State &_state; // reference to this instances state (held in the front-end)

127
libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp
Unescape View File

@ -2,17 +2,22 @@ @@ -2,17 +2,22 @@
#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_CAN.h>
#include <AP_Common/AP_Common.h>
#include <AP_Common/Semaphore.h>
#include <AP_Math/AP_Math.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
extern const AP_HAL::HAL& hal;
#include <uavcan/equipment/power/BatteryInfo.hpp>
extern const AP_HAL::HAL& hal;
#define debug_bm_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { printf(fmt, ##args); }} while (0)
UC_REGISTRY_BINDER(BattInfoCb, uavcan::equipment::power::BatteryInfo);
/// Constructor
AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params &params) :
AP_BattMonitor_Backend(mon, mon_state, params),
@ -22,58 +27,110 @@ AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor @@ -22,58 +27,110 @@ AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor
_state.healthy = false;
}
void AP_BattMonitor_UAVCAN::init()
void AP_BattMonitor_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
{
uint8_t can_num_drivers = AP::can().get_num_drivers();
if (ap_uavcan == nullptr) {
return;
}
for (uint8_t i = 0; i < can_num_drivers; i++) {
AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i);
if (ap_uavcan == nullptr) {
continue;
}
auto* node = ap_uavcan->get_node();
switch (_type) {
case UAVCAN_BATTERY_INFO:
if (ap_uavcan->register_BM_bi_listener_to_id(this, _params._serial_number)) {
debug_bm_uavcan(2, i, "UAVCAN BattMonitor BatteryInfo registered id: %d\n\r", _params._serial_number.get());
}
break;
}
uavcan::Subscriber<uavcan::equipment::power::BatteryInfo, BattInfoCb> *battinfo_listener;
battinfo_listener = new uavcan::Subscriber<uavcan::equipment::power::BatteryInfo, BattInfoCb>(*node);
// Backend Msg Handler
const int battinfo_listener_res = battinfo_listener->start(BattInfoCb(ap_uavcan, &handle_battery_info_trampoline));
if (battinfo_listener_res < 0) {
AP_HAL::panic("UAVCAN BatteryInfo subscriber start problem\n\r");
return;
}
}
// read - read the voltage and current
void AP_BattMonitor_UAVCAN::read()
AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id)
{
uint32_t tnow = AP_HAL::micros();
// timeout after 5 seconds
if ((tnow - _state.last_time_micros) > AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS) {
_state.healthy = false;
if (ap_uavcan == nullptr) {
return nullptr;
}
for (uint8_t i = 0; i < AP::battery()._num_instances; i++) {
if (AP::battery().drivers[i] == nullptr ||
AP::battery().get_type(i) != AP_BattMonitor_Params::BattMonitor_TYPE_UAVCAN_BatteryInfo) {
continue;
}
AP_BattMonitor_UAVCAN* driver = (AP_BattMonitor_UAVCAN*)AP::battery().drivers[i];
if (driver->_ap_uavcan == ap_uavcan && driver->_node_id == node_id) {
return driver;
}
}
// find empty uavcan driver
for (uint8_t i = 0; i < AP::battery()._num_instances; i++) {
if (AP::battery().drivers[i] != nullptr &&
AP::battery().get_type(i) == AP_BattMonitor_Params::BattMonitor_TYPE_UAVCAN_BatteryInfo) {
AP_BattMonitor_UAVCAN* batmon = (AP_BattMonitor_UAVCAN*)AP::battery().drivers[i];
batmon->_ap_uavcan = ap_uavcan;
batmon->_node_id = node_id;
batmon->init();
debug_bm_uavcan(2,
ap_uavcan->get_driver_index(),
"Registered BattMonitor Node %d on Bus %d\n",
node_id,
ap_uavcan->get_driver_index());
return batmon;
}
}
return nullptr;
}
void AP_BattMonitor_UAVCAN::handle_bi_msg(float voltage, float current, float temperature)
void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb)
{
_state.temperature = temperature;
_state.voltage = voltage;
_state.current_amps = current;
WITH_SEMAPHORE(_sem_battmon);
_interim_state.temperature = cb.msg->temperature;
_interim_state.voltage = cb.msg->voltage;
_interim_state.current_amps = cb.msg->current;
uint32_t tnow = AP_HAL::micros();
uint32_t dt = tnow - _state.last_time_micros;
uint32_t dt = tnow - _interim_state.last_time_micros;
// update total current drawn since startup
if (_state.last_time_micros != 0 && dt < 2000000) {
if (_interim_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.consumed_mah += mah;
_state.consumed_wh += 0.001f * mah * _state.voltage;
float mah = (float) ((double) _interim_state.current_amps * (double) dt * (double) 0.0000002778f);
_interim_state.consumed_mah += mah;
_interim_state.consumed_wh += 0.001f * mah * _interim_state.voltage;
}
// record time
_state.last_time_micros = tnow;
_interim_state.last_time_micros = tnow;
_state.healthy = true;
_interim_state.healthy = true;
}
void AP_BattMonitor_UAVCAN::handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb)
{
AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id);
if (driver == nullptr) {
return;
}
driver->handle_battery_info(cb);
}
// read - read the voltage and current
void AP_BattMonitor_UAVCAN::read()
{
uint32_t tnow = AP_HAL::micros();
// timeout after 5 seconds
if ((tnow - _interim_state.last_time_micros) > AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS) {
_interim_state.healthy = false;
}
// Copy over relevant states over to main state
WITH_SEMAPHORE(_sem_battmon);
_state.temperature = _interim_state.temperature;
_state.voltage = _interim_state.voltage;
_state.current_amps = _interim_state.current_amps;
_state.consumed_mah = _interim_state.consumed_mah;
_state.consumed_wh = _interim_state.consumed_wh;
_state.last_time_micros = _interim_state.last_time_micros;
_state.healthy = _interim_state.healthy;
}
#endif

24
libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h
Unescape View File

@ -1,15 +1,17 @@ @@ -1,15 +1,17 @@
#pragma once
#include <AP_UAVCAN/AP_UAVCAN.h>
#include "AP_BattMonitor.h"
#include "AP_BattMonitor_Backend.h"
#include <AP_UAVCAN/AP_UAVCAN.h>
#define AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS 5000000 // sensor becomes unhealthy if no successful readings for 5 seconds
class BattInfoCb;
class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend
{
public:
enum BattMonitor_UAVCAN_Type {
UAVCAN_BATTERY_INFO = 0
};
@ -17,17 +19,27 @@ public: @@ -17,17 +19,27 @@ public:
/// Constructor
AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params &params);
void init() override {}
/// 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;
static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
static AP_BattMonitor_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id);
static void handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb);
private:
void handle_battery_info(const BattInfoCb &cb);
protected:
AP_BattMonitor::BattMonitor_State _interim_state;
BattMonitor_UAVCAN_Type _type;
HAL_Semaphore _sem_battmon;
AP_UAVCAN* _ap_uavcan;
uint8_t _node_id;
};

Write Preview
Loading…
Cancel
Save