zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_UAVCAN: changes to servo bitmasks to support multiple instances, baro, compass, gps changes for several CAN interfaces

mission-4.1.18
Eugene Shamaev 8 years ago committed by Francisco Ferreira
parent
a7921a273b
commit
c6df13c795
2 changed files with 289 additions and 75 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 334
      libraries/AP_UAVCAN/AP_UAVCAN.cpp
  2. 30
      libraries/AP_UAVCAN/AP_UAVCAN.h

334
libraries/AP_UAVCAN/AP_UAVCAN.cpp
Unescape View File

@ -12,6 +12,9 @@
#include "AP_UAVCAN.h" #include "AP_UAVCAN.h"
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
// Zubax GPS and other GPS, baro, magnetic sensors // Zubax GPS and other GPS, baro, magnetic sensors
#include <uavcan/equipment/gnss/Fix.hpp> #include <uavcan/equipment/gnss/Fix.hpp>
#include <uavcan/equipment/gnss/Auxiliary.hpp> #include <uavcan/equipment/gnss/Auxiliary.hpp>
@ -23,11 +26,9 @@
#include <uavcan/equipment/actuator/Status.hpp> #include <uavcan/equipment/actuator/Status.hpp>
#include <uavcan/equipment/esc/RawCommand.hpp> #include <uavcan/equipment/esc/RawCommand.hpp>
#include <AP_BoardConfig/AP_BoardConfig.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
#define debug_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig::get_can_debug()) { hal.console->printf(fmt, ##args); }} while (0) #define debug_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig_CAN::get_can_debug()) { hal.console->printf(fmt, ##args); }} while (0)
// Translation of all messages from UAVCAN structures into AP structures is done // Translation of all messages from UAVCAN structures into AP structures is done
// in AP_UAVCAN and not in corresponding drivers. // in AP_UAVCAN and not in corresponding drivers.
@ -41,20 +42,33 @@ extern const AP_HAL::HAL& hal;
// table of user settable CAN bus parameters // table of user settable CAN bus parameters
const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
// @Param: NODE // @Param: NODE
// @DisplayName: UAVCAN node that is used for Ardupilot // @DisplayName: UAVCAN node that is used for this network
// @Description: UAVCAN node should be set implicitly // @Description: UAVCAN node should be set implicitly
// @Range: 1 250 // @Range: 1 250
// @User: Advanced // @User: Advanced
AP_GROUPINFO("NODE", 1, AP_UAVCAN, _uavcan_node, 10), AP_GROUPINFO("NODE", 1, AP_UAVCAN, _uavcan_node, 10),
// @Param: SRV_BM
// @DisplayName: RC Out channels to be transmitted as servo over UAVCAN
// @Description: Bitmask with one set for channel to be transmitted as a servo command over UAVCAN
// @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15
// @User: Advanced
AP_GROUPINFO("SRV_BM", 2, AP_UAVCAN, _servo_bm, 255),
// @Param: ESC_BM
// @DisplayName: RC Out channels to be transmitted as ESC over UAVCAN
// @Description: Bitmask with one set for channel to be transmitted as a ESC command over UAVCAN
// @Bitmask: 0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16
// @User: Advanced
AP_GROUPINFO("ESC_BM", 3, AP_UAVCAN, _esc_bm, 255),
AP_GROUPEND AP_GROUPEND
}; };
static uavcan::Subscriber<uavcan::equipment::gnss::Fix> *gnss_fix; static void gnss_fix_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>& msg, uint8_t mgr)
static void gnss_fix_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>& msg)
{ {
if (hal.can_mgr != nullptr) { if (hal.can_mgr[mgr] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN(); AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
if (ap_uavcan != nullptr) { if (ap_uavcan != nullptr) {
AP_GPS::GPS_State *state = ap_uavcan->find_gps_node(msg.getSrcNodeID().get()); AP_GPS::GPS_State *state = ap_uavcan->find_gps_node(msg.getSrcNodeID().get());
@ -153,11 +167,17 @@ static void gnss_fix_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::g
} }
} }
static uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary> *gnss_aux; static void gnss_fix_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>& msg)
static void gnss_aux_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary>& msg) { gnss_fix_cb(msg, 0); }
static void gnss_fix_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>& msg)
{ gnss_fix_cb(msg, 1); }
static void (*gnss_fix_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>& msg)
= { gnss_fix_cb0, gnss_fix_cb1 };
static void gnss_aux_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary>& msg, uint8_t mgr)
{ {
if (hal.can_mgr != nullptr) { if (hal.can_mgr[mgr] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN(); AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
if (ap_uavcan != nullptr) { if (ap_uavcan != nullptr) {
AP_GPS::GPS_State *state = ap_uavcan->find_gps_node(msg.getSrcNodeID().get()); AP_GPS::GPS_State *state = ap_uavcan->find_gps_node(msg.getSrcNodeID().get());
@ -174,11 +194,17 @@ static void gnss_aux_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::g
} }
} }
static uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength> *magnetic; static void gnss_aux_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary>& msg)
static void magnetic_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>& msg) { gnss_aux_cb(msg, 0); }
static void gnss_aux_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary>& msg)
{ gnss_aux_cb(msg, 1); }
static void (*gnss_aux_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary>& msg)
= { gnss_aux_cb0, gnss_aux_cb1 };
static void magnetic_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>& msg, uint8_t mgr)
{ {
if (hal.can_mgr != nullptr) { if (hal.can_mgr[mgr] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN(); AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
if (ap_uavcan != nullptr) { if (ap_uavcan != nullptr) {
AP_UAVCAN::Mag_Info *state = ap_uavcan->find_mag_node(msg.getSrcNodeID().get()); AP_UAVCAN::Mag_Info *state = ap_uavcan->find_mag_node(msg.getSrcNodeID().get());
if (state != nullptr) { if (state != nullptr) {
@ -193,13 +219,20 @@ static void magnetic_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::a
} }
} }
static uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure> *air_data_sp; static void magnetic_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>& msg)
static void air_data_sp_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure>& msg) { magnetic_cb(msg, 0); }
static void magnetic_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>& msg)
{ magnetic_cb(msg, 1); }
static void (*magnetic_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>& msg)
= { magnetic_cb0, magnetic_cb1 };
static void air_data_sp_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure>& msg, uint8_t mgr)
{ {
if (hal.can_mgr != nullptr) { if (hal.can_mgr[mgr] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN(); AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
if (ap_uavcan != nullptr) { if (ap_uavcan != nullptr) {
AP_UAVCAN::Baro_Info *state = ap_uavcan->find_baro_node(msg.getSrcNodeID().get()); AP_UAVCAN::Baro_Info *state = ap_uavcan->find_baro_node(msg.getSrcNodeID().get());
if (state != nullptr) { if (state != nullptr) {
state->pressure = msg.static_pressure; state->pressure = msg.static_pressure;
state->pressure_variance = msg.static_pressure_variance; state->pressure_variance = msg.static_pressure_variance;
@ -211,14 +244,21 @@ static void air_data_sp_cb(const uavcan::ReceivedDataStructure<uavcan::equipment
} }
} }
static void air_data_sp_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure>& msg)
{ air_data_sp_cb(msg, 0); }
static void air_data_sp_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure>& msg)
{ air_data_sp_cb(msg, 1); }
static void (*air_data_sp_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure>& msg)
= { air_data_sp_cb0, air_data_sp_cb1 };
// Temperature is not main parameter so do not update listeners when it is received // Temperature is not main parameter so do not update listeners when it is received
static uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature> *air_data_st; static void air_data_st_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature>& msg, uint8_t mgr)
static void air_data_st_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature>& msg)
{ {
if (hal.can_mgr != nullptr) { if (hal.can_mgr[mgr] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN(); AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
if (ap_uavcan != nullptr) { if (ap_uavcan != nullptr) {
AP_UAVCAN::Baro_Info *state = ap_uavcan->find_baro_node(msg.getSrcNodeID().get()); AP_UAVCAN::Baro_Info *state = ap_uavcan->find_baro_node(msg.getSrcNodeID().get());
if (state != nullptr) { if (state != nullptr) {
state->temperature = msg.static_temperature; state->temperature = msg.static_temperature;
state->temperature_variance = msg.static_temperature_variance; state->temperature_variance = msg.static_temperature_variance;
@ -227,12 +267,19 @@ static void air_data_st_cb(const uavcan::ReceivedDataStructure<uavcan::equipment
} }
} }
static void air_data_st_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature>& msg)
{ air_data_st_cb(msg, 0); }
static void air_data_st_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature>& msg)
{ air_data_st_cb(msg, 1); }
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 };
// publisher interfaces // publisher interfaces
static uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand> *act_out_array; static uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>* act_out_array[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::esc::RawCommand> *esc_raw; static uavcan::Publisher<uavcan::equipment::esc::RawCommand>* esc_raw[MAX_NUMBER_OF_CAN_DRIVERS];
AP_UAVCAN::AP_UAVCAN() : AP_UAVCAN::AP_UAVCAN() :
_initialized(false), _rco_armed(false), _rco_safety(false), _rc_out_sem(nullptr), _node_allocator( _node_allocator(
UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_SIZE) UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_SIZE)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
@ -242,28 +289,28 @@ AP_UAVCAN::AP_UAVCAN() :
} }
for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) { for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) {
_gps_nodes[i] = 255; _gps_nodes[i] = UINT8_MAX;
_gps_node_taken[i] = 0; _gps_node_taken[i] = 0;
} }
for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) { for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) {
_baro_nodes[i] = 255; _baro_nodes[i] = UINT8_MAX;
_baro_node_taken[i] = 0; _baro_node_taken[i] = 0;
} }
for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) { for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) {
_mag_nodes[i] = 255; _mag_nodes[i] = UINT8_MAX;
_mag_node_taken[i] = 0; _mag_node_taken[i] = 0;
} }
for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) {
_gps_listener_to_node[i] = 255; _gps_listener_to_node[i] = UINT8_MAX;
_gps_listeners[i] = nullptr; _gps_listeners[i] = nullptr;
_baro_listener_to_node[i] = 255; _baro_listener_to_node[i] = UINT8_MAX;
_baro_listeners[i] = nullptr; _baro_listeners[i] = nullptr;
_mag_listener_to_node[i] = 255; _mag_listener_to_node[i] = UINT8_MAX;
_mag_listeners[i] = nullptr; _mag_listeners[i] = nullptr;
} }
@ -278,8 +325,21 @@ AP_UAVCAN::~AP_UAVCAN()
bool AP_UAVCAN::try_init(void) bool AP_UAVCAN::try_init(void)
{ {
if (hal.can_mgr != nullptr) { if (_parent_can_mgr != nullptr) {
if (hal.can_mgr->is_initialized() && !_initialized) { if (_parent_can_mgr->is_initialized() && !_initialized) {
_uavcan_i = UINT8_MAX;
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
if (_parent_can_mgr == hal.can_mgr[i]) {
_uavcan_i = i;
break;
}
}
if(_uavcan_i == UINT8_MAX) {
return false;
}
auto *node = get_node(); auto *node = get_node();
if (node != nullptr) { if (node != nullptr) {
@ -287,7 +347,10 @@ bool AP_UAVCAN::try_init(void)
uavcan::NodeID self_node_id(_uavcan_node); uavcan::NodeID self_node_id(_uavcan_node);
node->setNodeID(self_node_id); node->setNodeID(self_node_id);
uavcan::NodeStatusProvider::NodeName name("org.ardupilot"); char ndname[20];
snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", _uavcan_i);
uavcan::NodeStatusProvider::NodeName name(ndname);
node->setName(name); node->setName(name);
uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion
@ -306,48 +369,54 @@ bool AP_UAVCAN::try_init(void)
debug_uavcan(1, "UAVCAN: node start problem\n\r"); debug_uavcan(1, "UAVCAN: node start problem\n\r");
} }
uavcan::Subscriber<uavcan::equipment::gnss::Fix> *gnss_fix;
gnss_fix = new uavcan::Subscriber<uavcan::equipment::gnss::Fix>(*node); gnss_fix = new uavcan::Subscriber<uavcan::equipment::gnss::Fix>(*node);
const int gnss_fix_start_res = gnss_fix->start(gnss_fix_cb);
const int gnss_fix_start_res = gnss_fix->start(gnss_fix_cb_arr[_uavcan_i]);
if (gnss_fix_start_res < 0) { if (gnss_fix_start_res < 0) {
debug_uavcan(1, "UAVCAN GNSS subscriber start problem\n\r"); debug_uavcan(1, "UAVCAN GNSS subscriber start problem\n\r");
return false; return false;
} }
uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary> *gnss_aux;
gnss_aux = new uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary>(*node); gnss_aux = new uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary>(*node);
const int gnss_aux_start_res = gnss_aux->start(gnss_aux_cb); const int gnss_aux_start_res = gnss_aux->start(gnss_aux_cb_arr[_uavcan_i]);
if (gnss_aux_start_res < 0) { if (gnss_aux_start_res < 0) {
debug_uavcan(1, "UAVCAN GNSS Aux subscriber start problem\n\r"); debug_uavcan(1, "UAVCAN GNSS Aux subscriber start problem\n\r");
return false; return false;
} }
uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength> *magnetic;
magnetic = new uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength>(*node); magnetic = new uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength>(*node);
const int magnetic_start_res = magnetic->start(magnetic_cb); const int magnetic_start_res = magnetic->start(magnetic_cb_arr[_uavcan_i]);
if (magnetic_start_res < 0) { if (magnetic_start_res < 0) {
debug_uavcan(1, "UAVCAN Compass subscriber start problem\n\r"); debug_uavcan(1, "UAVCAN Compass subscriber start problem\n\r");
return false; return false;
} }
uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure> *air_data_sp;
air_data_sp = new uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure>(*node); air_data_sp = new uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure>(*node);
const int air_data_sp_start_res = air_data_sp->start(air_data_sp_cb); const int air_data_sp_start_res = air_data_sp->start(air_data_sp_cb_arr[_uavcan_i]);
if (air_data_sp_start_res < 0) { if (air_data_sp_start_res < 0) {
debug_uavcan(1, "UAVCAN Baro subscriber start problem\n\r"); debug_uavcan(1, "UAVCAN Baro subscriber start problem\n\r");
return false; return false;
} }
uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature> *air_data_st;
air_data_st = new uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature>(*node); air_data_st = new uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature>(*node);
const int air_data_st_start_res = air_data_st->start(air_data_st_cb); const int air_data_st_start_res = air_data_st->start(air_data_st_cb_arr[_uavcan_i]);
if (air_data_st_start_res < 0) { if (air_data_st_start_res < 0) {
debug_uavcan(1, "UAVCAN Temperature subscriber start problem\n\r"); debug_uavcan(1, "UAVCAN Temperature subscriber start problem\n\r");
return false; return false;
} }
act_out_array = new uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>(*node); act_out_array[_uavcan_i] = new uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>(*node);
act_out_array->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); act_out_array[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
act_out_array->setPriority(uavcan::TransferPriority::OneLowerThanHighest); act_out_array[_uavcan_i]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
esc_raw = new uavcan::Publisher<uavcan::equipment::esc::RawCommand>(*node); esc_raw[_uavcan_i] = new uavcan::Publisher<uavcan::equipment::esc::RawCommand>(*node);
esc_raw->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); esc_raw[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
esc_raw->setPriority(uavcan::TransferPriority::OneLowerThanHighest); esc_raw[_uavcan_i]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
/* /*
* Informing other nodes that we're ready to work. * Informing other nodes that we're ready to work.
@ -388,13 +457,11 @@ void AP_UAVCAN::rc_out_sem_give()
void AP_UAVCAN::do_cyclic(void) void AP_UAVCAN::do_cyclic(void)
{ {
uint32_t _servo_bm = SRV_Channels::get_can_servo_bm();
uint32_t _esc_bm = SRV_Channels::get_can_esc_bm();
if (_initialized) { if (_initialized) {
auto *node = get_node(); auto *node = get_node();
const int error = node->spin(uavcan::MonotonicDuration::fromMSec(1)); const int error = node->spin(uavcan::MonotonicDuration::fromMSec(1));
if (error < 0) { if (error < 0) {
hal.scheduler->delay_microseconds(1000); hal.scheduler->delay_microseconds(1000);
} else { } else {
@ -440,7 +507,7 @@ void AP_UAVCAN::do_cyclic(void)
} }
if (i > 0) { if (i > 0) {
act_out_array->broadcast(msg); act_out_array[_uavcan_i]->broadcast(msg);
if (i == 15) { if (i == 15) {
repeat_send = true; repeat_send = true;
@ -488,7 +555,7 @@ void AP_UAVCAN::do_cyclic(void)
k++; k++;
} }
esc_raw->broadcast(esc_msg); esc_raw[_uavcan_i]->broadcast(esc_msg);
} }
} }
} }
@ -501,6 +568,8 @@ void AP_UAVCAN::do_cyclic(void)
rc_out_sem_give(); rc_out_sem_give();
} }
} }
} else {
hal.scheduler->delay_microseconds(1000);
} }
} }
@ -511,11 +580,11 @@ uavcan::ISystemClock & AP_UAVCAN::get_system_clock()
uavcan::ICanDriver * AP_UAVCAN::get_can_driver() uavcan::ICanDriver * AP_UAVCAN::get_can_driver()
{ {
if (hal.can_mgr != nullptr) { if (_parent_can_mgr != nullptr) {
if (hal.can_mgr->is_initialized() == false) { if (_parent_can_mgr->is_initialized() == false) {
return nullptr; return nullptr;
} else { } else {
return hal.can_mgr; return _parent_can_mgr;
} }
} }
return nullptr; return nullptr;
@ -569,9 +638,20 @@ void AP_UAVCAN::rco_write(uint16_t pulse_len, uint8_t ch)
_rco_conf[ch].active = true; _rco_conf[ch].active = true;
} }
uint8_t AP_UAVCAN::find_gps_without_listener(void)
{
for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) {
if (_gps_listeners[i] == nullptr && _gps_nodes[i] != UINT8_MAX) {
return _gps_nodes[i];
}
}
return UINT8_MAX;
}
uint8_t AP_UAVCAN::register_gps_listener(AP_GPS_Backend* new_listener, uint8_t preferred_channel) uint8_t AP_UAVCAN::register_gps_listener(AP_GPS_Backend* new_listener, uint8_t preferred_channel)
{ {
uint8_t sel_place = 255, ret = 0; uint8_t sel_place = UINT8_MAX, ret = 0;
for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) {
if (_gps_listeners[i] == nullptr) { if (_gps_listeners[i] == nullptr) {
sel_place = i; sel_place = i;
@ -579,7 +659,7 @@ uint8_t AP_UAVCAN::register_gps_listener(AP_GPS_Backend* new_listener, uint8_t p
} }
} }
if (sel_place != 255) { if (sel_place != UINT8_MAX) {
if (preferred_channel != 0) { if (preferred_channel != 0) {
if (preferred_channel <= AP_UAVCAN_MAX_GPS_NODES) { if (preferred_channel <= AP_UAVCAN_MAX_GPS_NODES) {
_gps_listeners[sel_place] = new_listener; _gps_listeners[sel_place] = new_listener;
@ -607,6 +687,34 @@ uint8_t AP_UAVCAN::register_gps_listener(AP_GPS_Backend* new_listener, uint8_t p
return ret; return ret;
} }
uint8_t AP_UAVCAN::register_gps_listener_to_node(AP_GPS_Backend* new_listener, uint8_t node)
{
uint8_t sel_place = UINT8_MAX, ret = 0;
for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) {
if (_gps_listeners[i] == nullptr) {
sel_place = i;
break;
}
}
if (sel_place != UINT8_MAX) {
for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) {
if (_gps_nodes[i] == node) {
_gps_listeners[sel_place] = new_listener;
_gps_listener_to_node[sel_place] = i;
_gps_node_taken[i]++;
ret = i + 1;
debug_uavcan(2, "reg_GPS place:%d, chan: %d\n\r", sel_place, i);
break;
}
}
}
return ret;
}
void AP_UAVCAN::remove_gps_listener(AP_GPS_Backend* rem_listener) void AP_UAVCAN::remove_gps_listener(AP_GPS_Backend* rem_listener)
{ {
// Check for all listeners and compare pointers // Check for all listeners and compare pointers
@ -618,7 +726,7 @@ void AP_UAVCAN::remove_gps_listener(AP_GPS_Backend* rem_listener)
if (_gps_node_taken[_gps_listener_to_node[i]] > 0) { if (_gps_node_taken[_gps_listener_to_node[i]] > 0) {
_gps_node_taken[_gps_listener_to_node[i]]--; _gps_node_taken[_gps_listener_to_node[i]]--;
} }
_gps_listener_to_node[i] = 255; _gps_listener_to_node[i] = UINT8_MAX;
} }
} }
} }
@ -634,7 +742,7 @@ AP_GPS::GPS_State *AP_UAVCAN::find_gps_node(uint8_t node)
// If not - try to find free space for it // If not - try to find free space for it
for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) { for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) {
if (_gps_nodes[i] == 255) { if (_gps_nodes[i] == UINT8_MAX) {
_gps_nodes[i] = node; _gps_nodes[i] = node;
return &_gps_node_state[i]; return &_gps_node_state[i];
} }
@ -660,7 +768,7 @@ void AP_UAVCAN::update_gps_state(uint8_t node)
uint8_t AP_UAVCAN::register_baro_listener(AP_Baro_Backend* new_listener, uint8_t preferred_channel) uint8_t AP_UAVCAN::register_baro_listener(AP_Baro_Backend* new_listener, uint8_t preferred_channel)
{ {
uint8_t sel_place = 255, ret = 0; uint8_t sel_place = UINT8_MAX, ret = 0;
for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) {
if (_baro_listeners[i] == nullptr) { if (_baro_listeners[i] == nullptr) {
@ -669,7 +777,7 @@ uint8_t AP_UAVCAN::register_baro_listener(AP_Baro_Backend* new_listener, uint8_t
} }
} }
if (sel_place != 255) { if (sel_place != UINT8_MAX) {
if (preferred_channel != 0) { if (preferred_channel != 0) {
if (preferred_channel < AP_UAVCAN_MAX_BARO_NODES) { if (preferred_channel < AP_UAVCAN_MAX_BARO_NODES) {
_baro_listeners[sel_place] = new_listener; _baro_listeners[sel_place] = new_listener;
@ -697,6 +805,35 @@ uint8_t AP_UAVCAN::register_baro_listener(AP_Baro_Backend* new_listener, uint8_t
return ret; return ret;
} }
uint8_t AP_UAVCAN::register_baro_listener_to_node(AP_Baro_Backend* new_listener, uint8_t node)
{
uint8_t sel_place = UINT8_MAX, ret = 0;
for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) {
if (_baro_listeners[i] == nullptr) {
sel_place = i;
break;
}
}
if (sel_place != UINT8_MAX) {
for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) {
if (_baro_nodes[i] == node) {
_baro_listeners[sel_place] = new_listener;
_baro_listener_to_node[sel_place] = i;
_baro_node_taken[i]++;
ret = i + 1;
debug_uavcan(2, "reg_BARO place:%d, chan: %d\n\r", sel_place, i);
break;
}
}
}
return ret;
}
void AP_UAVCAN::remove_baro_listener(AP_Baro_Backend* rem_listener) void AP_UAVCAN::remove_baro_listener(AP_Baro_Backend* rem_listener)
{ {
// Check for all listeners and compare pointers // Check for all listeners and compare pointers
@ -708,7 +845,7 @@ void AP_UAVCAN::remove_baro_listener(AP_Baro_Backend* rem_listener)
if (_baro_node_taken[_baro_listener_to_node[i]] > 0) { if (_baro_node_taken[_baro_listener_to_node[i]] > 0) {
_baro_node_taken[_baro_listener_to_node[i]]--; _baro_node_taken[_baro_listener_to_node[i]]--;
} }
_baro_listener_to_node[i] = 255; _baro_listener_to_node[i] = UINT8_MAX;
} }
} }
} }
@ -724,7 +861,8 @@ AP_UAVCAN::Baro_Info *AP_UAVCAN::find_baro_node(uint8_t node)
// If not - try to find free space for it // If not - try to find free space for it
for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) { for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) {
if (_baro_nodes[i] == 255) { if (_baro_nodes[i] == UINT8_MAX) {
_baro_nodes[i] = node; _baro_nodes[i] = node;
return &_baro_node_state[i]; return &_baro_node_state[i];
} }
@ -748,9 +886,25 @@ void AP_UAVCAN::update_baro_state(uint8_t node)
} }
} }
/*
* Find discovered not taken baro node with smallest node ID
*/
uint8_t AP_UAVCAN::find_smallest_free_baro_node()
{
uint8_t ret = UINT8_MAX;
for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) {
if (_baro_node_taken[i] == 0) {
ret = MIN(ret, _baro_nodes[i]);
}
}
return ret;
}
uint8_t AP_UAVCAN::register_mag_listener(AP_Compass_Backend* new_listener, uint8_t preferred_channel) uint8_t AP_UAVCAN::register_mag_listener(AP_Compass_Backend* new_listener, uint8_t preferred_channel)
{ {
uint8_t sel_place = 255, ret = 0; uint8_t sel_place = UINT8_MAX, ret = 0;
for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) {
if (_mag_listeners[i] == nullptr) { if (_mag_listeners[i] == nullptr) {
sel_place = i; sel_place = i;
@ -758,7 +912,7 @@ uint8_t AP_UAVCAN::register_mag_listener(AP_Compass_Backend* new_listener, uint8
} }
} }
if (sel_place != 255) { if (sel_place != UINT8_MAX) {
if (preferred_channel != 0) { if (preferred_channel != 0) {
if (preferred_channel < AP_UAVCAN_MAX_MAG_NODES) { if (preferred_channel < AP_UAVCAN_MAX_MAG_NODES) {
_mag_listeners[sel_place] = new_listener; _mag_listeners[sel_place] = new_listener;
@ -786,6 +940,34 @@ uint8_t AP_UAVCAN::register_mag_listener(AP_Compass_Backend* new_listener, uint8
return ret; return ret;
} }
uint8_t AP_UAVCAN::register_mag_listener_to_node(AP_Compass_Backend* new_listener, uint8_t node)
{
uint8_t sel_place = UINT8_MAX, ret = 0;
for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) {
if (_mag_listeners[i] == nullptr) {
sel_place = i;
break;
}
}
if (sel_place != UINT8_MAX) {
for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) {
if (_mag_nodes[i] == node) {
_mag_listeners[sel_place] = new_listener;
_mag_listener_to_node[sel_place] = i;
_mag_node_taken[i]++;
ret = i + 1;
debug_uavcan(2, "reg_MAG place:%d, chan: %d\n\r", sel_place, i);
break;
}
}
}
return ret;
}
void AP_UAVCAN::remove_mag_listener(AP_Compass_Backend* rem_listener) void AP_UAVCAN::remove_mag_listener(AP_Compass_Backend* rem_listener)
{ {
// Check for all listeners and compare pointers // Check for all listeners and compare pointers
@ -797,7 +979,7 @@ void AP_UAVCAN::remove_mag_listener(AP_Compass_Backend* rem_listener)
if (_mag_node_taken[_mag_listener_to_node[i]] > 0) { if (_mag_node_taken[_mag_listener_to_node[i]] > 0) {
_mag_node_taken[_mag_listener_to_node[i]]--; _mag_node_taken[_mag_listener_to_node[i]]--;
} }
_mag_listener_to_node[i] = 255; _mag_listener_to_node[i] = UINT8_MAX;
} }
} }
} }
@ -813,7 +995,7 @@ AP_UAVCAN::Mag_Info *AP_UAVCAN::find_mag_node(uint8_t node)
// If not - try to find free space for it // If not - try to find free space for it
for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) { for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) {
if (_mag_nodes[i] == 255) { if (_mag_nodes[i] == UINT8_MAX) {
_mag_nodes[i] = node; _mag_nodes[i] = node;
return &_mag_node_state[i]; return &_mag_node_state[i];
} }
@ -823,6 +1005,22 @@ AP_UAVCAN::Mag_Info *AP_UAVCAN::find_mag_node(uint8_t node)
return nullptr; return nullptr;
} }
/*
* Find discovered not taken mag node with smallest node ID
*/
uint8_t AP_UAVCAN::find_smallest_free_mag_node()
{
uint8_t ret = UINT8_MAX;
for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) {
if (_mag_node_taken[i] == 0) {
ret = MIN(ret, _mag_nodes[i]);
}
}
return ret;
}
void AP_UAVCAN::update_mag_state(uint8_t node) void AP_UAVCAN::update_mag_state(uint8_t node)
{ {
// Go through all listeners of specified node and call their's update methods // Go through all listeners of specified node and call their's update methods

30
libraries/AP_UAVCAN/AP_UAVCAN.h
Unescape View File

@ -1,5 +1,4 @@
/* /*
* AP_UAVCAN.h
* *
* Author: Eugene Shamaev * Author: Eugene Shamaev
*/ */
@ -54,8 +53,12 @@ public:
// if preferred_channel > 0 then listener will be added to specific channel // if preferred_channel > 0 then listener will be added to specific channel
// return value is the number of assigned channel or 0 if fault // return value is the number of assigned channel or 0 if fault
// channel numbering starts from 1 // channel numbering starts from 1
uint8_t register_gps_listener(AP_GPS_Backend* new_listener, uint8_t register_gps_listener(AP_GPS_Backend* new_listener, uint8_t preferred_channel);
uint8_t preferred_channel);
uint8_t register_gps_listener_to_node(AP_GPS_Backend* new_listener, uint8_t node);
uint8_t find_gps_without_listener(void);
// Removes specified listener from all nodes // Removes specified listener from all nodes
void remove_gps_listener(AP_GPS_Backend* rem_listener); void remove_gps_listener(AP_GPS_Backend* rem_listener);
@ -73,20 +76,22 @@ public:
float temperature_variance; float temperature_variance;
}; };
uint8_t register_baro_listener(AP_Baro_Backend* new_listener, uint8_t register_baro_listener(AP_Baro_Backend* new_listener, uint8_t preferred_channel);
uint8_t preferred_channel); uint8_t register_baro_listener_to_node(AP_Baro_Backend* new_listener, uint8_t node);
void remove_baro_listener(AP_Baro_Backend* rem_listener); void remove_baro_listener(AP_Baro_Backend* rem_listener);
Baro_Info *find_baro_node(uint8_t node); Baro_Info *find_baro_node(uint8_t node);
uint8_t find_smallest_free_baro_node();
void update_baro_state(uint8_t node); void update_baro_state(uint8_t node);
struct Mag_Info { struct Mag_Info {
Vector3f mag_vector; Vector3f mag_vector;
}; };
uint8_t register_mag_listener(AP_Compass_Backend* new_listener, uint8_t register_mag_listener(AP_Compass_Backend* new_listener, uint8_t preferred_channel);
uint8_t preferred_channel);
void remove_mag_listener(AP_Compass_Backend* rem_listener); void remove_mag_listener(AP_Compass_Backend* rem_listener);
Mag_Info *find_mag_node(uint8_t node); Mag_Info *find_mag_node(uint8_t node);
uint8_t find_smallest_free_mag_node();
uint8_t register_mag_listener_to_node(AP_Compass_Backend* new_listener, uint8_t node);
void update_mag_state(uint8_t node); void update_mag_state(uint8_t node);
// synchronization for RC output // synchronization for RC output
@ -191,6 +196,12 @@ private:
uavcan::HeapBasedPoolAllocator<UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator; uavcan::HeapBasedPoolAllocator<UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator;
AP_Int8 _uavcan_node; AP_Int8 _uavcan_node;
AP_Int32 _servo_bm;
AP_Int32 _esc_bm;
uint8_t _uavcan_i;
AP_HAL::CANManager* _parent_can_mgr;
public: public:
void do_cyclic(void); void do_cyclic(void);
@ -202,6 +213,11 @@ public:
void rco_force_safety_off(void); void rco_force_safety_off(void);
void rco_arm_actuators(bool arm); void rco_arm_actuators(bool arm);
void rco_write(uint16_t pulse_len, uint8_t ch); void rco_write(uint16_t pulse_len, uint8_t ch);
void set_parent_can_mgr(AP_HAL::CANManager* parent_can_mgr)
{
_parent_can_mgr = parent_can_mgr;
}
}; };
#endif /* AP_UAVCAN_H_ */ #endif /* AP_UAVCAN_H_ */

Write Preview
Loading…
Cancel
Save