Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: UAVCAN battery information added

This commit is contained in:
DOMINATOR\Eugene 2018-02-17 00:32:50 +02:00 committed by Tom Pittenger
parent fcccc0174b
commit 844ed611c6
2 changed files with 165 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

142
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -18,15 +18,20 @@
// Zubax GPS and other GPS, baro, magnetic sensors
#include <uavcan/equipment/gnss/Fix.hpp>
#include <uavcan/equipment/gnss/Auxiliary.hpp>
#include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
#include <uavcan/equipment/ahrs/MagneticFieldStrength2.hpp>
#include <uavcan/equipment/air_data/StaticPressure.hpp>
#include <uavcan/equipment/air_data/StaticTemperature.hpp>
#include <uavcan/equipment/actuator/ArrayCommand.hpp>
#include <uavcan/equipment/actuator/Command.hpp>
#include <uavcan/equipment/actuator/Status.hpp>
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <uavcan/equipment/power/BatteryInfo.hpp>
extern const AP_HAL::HAL& hal;
#define debug_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig_CAN::get_can_debug()) { hal.console->printf(fmt, ##args); }} while (0)
@ -300,6 +305,35 @@ static void air_data_st_cb1(const uavcan::ReceivedDataStructure<uavcan::equipmen
static void (*air_data_st_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature>& msg)
= { air_data_st_cb0, air_data_st_cb1 };
static void battery_info_st_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo>& msg, uint8_t mgr)
{
if (hal.can_mgr[mgr] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
if (ap_uavcan != nullptr) {
AP_UAVCAN::BatteryInfo_Info *state = ap_uavcan->find_bi_id((uint16_t) msg.battery_id);
if (state != nullptr) {
state->temperature = msg.temperature;
state->voltage = msg.voltage;
state->current = msg.current;
state->full_charge_capacity_wh = msg.full_charge_capacity_wh;
state->remaining_capacity_wh = msg.remaining_capacity_wh;
state->status_flags = msg.status_flags;
// after all is filled, update all listeners with new data
ap_uavcan->update_bi_state((uint16_t) msg.battery_id);
}
}
}
}
static void battery_info_st_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo>& msg)
{ battery_info_st_cb(msg, 0); }
static void battery_info_st_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo>& msg)
{ battery_info_st_cb(msg, 1); }
static void (*battery_info_st_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo>& msg)
= { battery_info_st_cb0, battery_info_st_cb1 };
// publisher interfaces
static uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>* act_out_array[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::esc::RawCommand>* esc_raw[MAX_NUMBER_OF_CAN_DRIVERS];
@ -342,6 +376,13 @@ AP_UAVCAN::AP_UAVCAN() :
_mag_listener_sensor_ids[i] = 0;
}
for (uint8_t i = 0; i < AP_UAVCAN_MAX_BI_NUMBER; i++) {
_bi_id[i] = UINT8_MAX;
_bi_id_taken[i] = 0;
_bi_BM_listener_to_id[i] = UINT8_MAX;
_bi_BM_listeners[i] = nullptr;
}
_rc_out_sem = hal.util->new_semaphore();
debug_uavcan(2, "AP_UAVCAN constructed\n\r");
@ -446,6 +487,14 @@ bool AP_UAVCAN::try_init(void)
return false;
}
uavcan::Subscriber<uavcan::equipment::power::BatteryInfo> *battery_info_st;
battery_info_st = new uavcan::Subscriber<uavcan::equipment::power::BatteryInfo>(*node);
const int battery_info_start_res = battery_info_st->start(battery_info_st_cb_arr[_uavcan_i]);
if (battery_info_start_res < 0) {
debug_uavcan(1, "UAVCAN BatteryInfo subscriber start problem\n\r");
return false;
}
act_out_array[_uavcan_i] = new uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>(*node);
act_out_array[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
act_out_array[_uavcan_i]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
@ -882,7 +931,6 @@ uint8_t AP_UAVCAN::register_baro_listener_to_node(AP_Baro_Backend* new_listener,
return ret;
}
void AP_UAVCAN::remove_baro_listener(AP_Baro_Backend* rem_listener)
{
// Check for all listeners and compare pointers
@ -1116,4 +1164,96 @@ void AP_UAVCAN::update_mag_state(uint8_t node, uint8_t sensor_id)
}
}
uint8_t AP_UAVCAN::register_BM_bi_listener_to_id(AP_BattMonitor_Backend* new_listener, uint8_t id)
{
uint8_t sel_place = UINT8_MAX, ret = 0;
for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) {
if (_bi_BM_listeners[i] == nullptr) {
sel_place = i;
break;
}
}
if (sel_place != UINT8_MAX) {
for (uint8_t i = 0; i < AP_UAVCAN_MAX_BI_NUMBER; i++) {
if (_bi_id[i] == id) {
_bi_BM_listeners[sel_place] = new_listener;
_bi_BM_listener_to_id[sel_place] = i;
_bi_id_taken[i]++;
ret = i + 1;
debug_uavcan(2, "reg_BI place:%d, chan: %d\n\r", sel_place, i);
break;
}
}
}
return ret;
}
void AP_UAVCAN::remove_BM_bi_listener(AP_BattMonitor_Backend* rem_listener)
{
// Check for all listeners and compare pointers
for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) {
if (_bi_BM_listeners[i] == rem_listener) {
_bi_BM_listeners[i] = nullptr;
// Also decrement usage counter and reset listening node
if (_bi_id_taken[_bi_BM_listener_to_id[i]] > 0) {
_bi_id_taken[_bi_BM_listener_to_id[i]]--;
}
_bi_BM_listener_to_id[i] = UINT8_MAX;
}
}
}
AP_UAVCAN::BatteryInfo_Info *AP_UAVCAN::find_bi_id(uint8_t id)
{
// Check if such node is already defined
for (uint8_t i = 0; i < AP_UAVCAN_MAX_BI_NUMBER; i++) {
if (_bi_id[i] == id) {
return &_bi_id_state[i];
}
}
// If not - try to find free space for it
for (uint8_t i = 0; i < AP_UAVCAN_MAX_BI_NUMBER; i++) {
if (_bi_id[i] == UINT8_MAX) {
_bi_id[i] = id;
return &_bi_id_state[i];
}
}
// If no space is left - return nullptr
return nullptr;
}
uint8_t AP_UAVCAN::find_smallest_free_bi_id()
{
uint8_t ret = UINT8_MAX;
for (uint8_t i = 0; i < AP_UAVCAN_MAX_BI_NUMBER; i++) {
if (_bi_id_taken[i] == 0) {
ret = MIN(ret, _bi_id[i]);
}
}
return ret;
}
void AP_UAVCAN::update_bi_state(uint8_t id)
{
// Go through all listeners of specified node and call their's update methods
for (uint8_t i = 0; i < AP_UAVCAN_MAX_BI_NUMBER; i++) {
if (_bi_id[i] == id) {
for (uint8_t j = 0; j < AP_UAVCAN_MAX_LISTENERS; j++) {
if (_bi_BM_listener_to_id[j] == i) {
_bi_BM_listeners[j]->handle_bi_msg(_bi_id_state[i].voltage, _bi_id_state[i].current, _bi_id_state[i].temperature);
}
}
}
}
}
#endif // HAL_WITH_UAVCAN

24
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -15,6 +15,7 @@
#include <AP_GPS/GPS_Backend.h>
#include <AP_Baro/AP_Baro_Backend.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_BattMonitor/AP_BattMonitor_Backend.h>
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
@ -34,6 +35,7 @@
#define AP_UAVCAN_MAX_GPS_NODES 4
#define AP_UAVCAN_MAX_MAG_NODES 4
#define AP_UAVCAN_MAX_BARO_NODES 4
#define AP_UAVCAN_MAX_BI_NUMBER 4
#define AP_UAVCAN_SW_VERS_MAJOR 1
#define AP_UAVCAN_SW_VERS_MINOR 0
@ -94,6 +96,21 @@ public:
uint8_t register_mag_listener_to_node(AP_Compass_Backend* new_listener, uint8_t node);
void update_mag_state(uint8_t node, uint8_t sensor_id);
struct BatteryInfo_Info {
float temperature;
float voltage;
float current;
float remaining_capacity_wh;
float full_charge_capacity_wh;
uint8_t status_flags;
};
uint8_t register_BM_bi_listener_to_id(AP_BattMonitor_Backend* new_listener, uint8_t id);
void remove_BM_bi_listener(AP_BattMonitor_Backend* rem_listener);
BatteryInfo_Info *find_bi_id(uint8_t id);
uint8_t find_smallest_free_bi_id();
void update_bi_state(uint8_t id);
// synchronization for RC output
bool rc_out_sem_take();
void rc_out_sem_give();
@ -132,6 +149,13 @@ private:
AP_Compass_Backend* _mag_listeners[AP_UAVCAN_MAX_LISTENERS];
uint8_t _mag_listener_sensor_ids[AP_UAVCAN_MAX_LISTENERS];
// ------------------------- BatteryInfo
uint16_t _bi_id[AP_UAVCAN_MAX_BI_NUMBER];
uint16_t _bi_id_taken[AP_UAVCAN_MAX_BI_NUMBER];
BatteryInfo_Info _bi_id_state[AP_UAVCAN_MAX_BI_NUMBER];
uint16_t _bi_BM_listener_to_id[AP_UAVCAN_MAX_LISTENERS];
AP_BattMonitor_Backend* _bi_BM_listeners[AP_UAVCAN_MAX_LISTENERS];
struct {
uint16_t pulse;
uint16_t safety_pulse;