diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 9c61d06e99..1fd610cbc6 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -37,7 +37,7 @@ extern const AP_HAL::HAL& hal; -#define debug_uavcan(level_debug, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(_uavcan_i)) { hal.console->printf(fmt, ##args); }} while (0) +#define debug_uavcan(level_debug, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(_driver_index)) { printf(fmt, ##args); }} while (0) // Translation of all messages from UAVCAN structures into AP structures is done // in AP_UAVCAN and not in corresponding drivers. @@ -419,47 +419,38 @@ AP_UAVCAN::~AP_UAVCAN() { } -bool AP_UAVCAN::try_init(void) +void AP_UAVCAN::init(uint8_t driver_index) { - if (_parent_can_mgr == nullptr) { - return false; - } - if (_initialized) { - return true; - } - - if (!_parent_can_mgr->is_initialized()) { - return false; - } - - _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; - } + return; } - if(_uavcan_i == UINT8_MAX) { - return false; + _driver_index = driver_index; + + AP_HAL::CANManager* can_mgr = hal.can_mgr[driver_index]; + if (can_mgr == nullptr) { + return; + } + + if (!can_mgr->is_initialized()) { + return; } auto *node = get_node(); if (node == nullptr) { - return false; + return; } - + if (node->isStarted()) { - return false; + return; } - + uavcan::NodeID self_node_id(_uavcan_node); node->setNodeID(self_node_id); char ndname[20]; - snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", _uavcan_i); + snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index); uavcan::NodeStatusProvider::NodeName name(ndname); node->setName(name); @@ -478,76 +469,77 @@ bool AP_UAVCAN::try_init(void) const int node_start_res = node->start(); if (node_start_res < 0) { debug_uavcan(1, "UAVCAN: node start problem\n\r"); + return; } uavcan::Subscriber *gnss_fix; gnss_fix = new uavcan::Subscriber(*node); - const int gnss_fix_start_res = gnss_fix->start(gnss_fix_cb_arr[_uavcan_i]); + const int gnss_fix_start_res = gnss_fix->start(gnss_fix_cb_arr[driver_index]); if (gnss_fix_start_res < 0) { debug_uavcan(1, "UAVCAN GNSS subscriber start problem\n\r"); - return false; + return; } uavcan::Subscriber *gnss_aux; gnss_aux = new uavcan::Subscriber(*node); - const int gnss_aux_start_res = gnss_aux->start(gnss_aux_cb_arr[_uavcan_i]); + const int gnss_aux_start_res = gnss_aux->start(gnss_aux_cb_arr[driver_index]); if (gnss_aux_start_res < 0) { debug_uavcan(1, "UAVCAN GNSS Aux subscriber start problem\n\r"); - return false; + return; } uavcan::Subscriber *magnetic; magnetic = new uavcan::Subscriber(*node); - const int magnetic_start_res = magnetic->start(magnetic_cb_arr[_uavcan_i]); + const int magnetic_start_res = magnetic->start(magnetic_cb_arr[driver_index]); if (magnetic_start_res < 0) { debug_uavcan(1, "UAVCAN Compass subscriber start problem\n\r"); - return false; + return; } uavcan::Subscriber *magnetic2; magnetic2 = new uavcan::Subscriber(*node); - const int magnetic_start_res_2 = magnetic2->start(magnetic_cb_2_arr[_uavcan_i]); + const int magnetic_start_res_2 = magnetic2->start(magnetic_cb_2_arr[driver_index]); if (magnetic_start_res_2 < 0) { debug_uavcan(1, "UAVCAN Compass for multiple mags subscriber start problem\n\r"); - return false; + return; } uavcan::Subscriber *air_data_sp; air_data_sp = new uavcan::Subscriber(*node); - const int air_data_sp_start_res = air_data_sp->start(air_data_sp_cb_arr[_uavcan_i]); + const int air_data_sp_start_res = air_data_sp->start(air_data_sp_cb_arr[driver_index]); if (air_data_sp_start_res < 0) { debug_uavcan(1, "UAVCAN Baro subscriber start problem\n\r"); - return false; + return; } - + uavcan::Subscriber *air_data_st; air_data_st = new uavcan::Subscriber(*node); - const int air_data_st_start_res = air_data_st->start(air_data_st_cb_arr[_uavcan_i]); + const int air_data_st_start_res = air_data_st->start(air_data_st_cb_arr[driver_index]); if (air_data_st_start_res < 0) { debug_uavcan(1, "UAVCAN Temperature subscriber start problem\n\r"); - return false; + return; } - + uavcan::Subscriber *battery_info_st; battery_info_st = new uavcan::Subscriber(*node); - const int battery_info_start_res = battery_info_st->start(battery_info_st_cb_arr[_uavcan_i]); + const int battery_info_start_res = battery_info_st->start(battery_info_st_cb_arr[driver_index]); if (battery_info_start_res < 0) { debug_uavcan(1, "UAVCAN BatteryInfo subscriber start problem\n\r"); - return false; + return; } - act_out_array[_uavcan_i] = new uavcan::Publisher(*node); - act_out_array[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); - act_out_array[_uavcan_i]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); + act_out_array[driver_index] = new uavcan::Publisher(*node); + act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); + act_out_array[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); - esc_raw[_uavcan_i] = new uavcan::Publisher(*node); - esc_raw[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); - esc_raw[_uavcan_i]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); + esc_raw[driver_index] = new uavcan::Publisher(*node); + esc_raw[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); + esc_raw[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); - rgb_led[_uavcan_i] = new uavcan::Publisher(*node); - rgb_led[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - rgb_led[_uavcan_i]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + rgb_led[driver_index] = new uavcan::Publisher(*node); + rgb_led[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); + rgb_led[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); _led_conf.devices_count = 0; @@ -557,11 +549,19 @@ bool AP_UAVCAN::try_init(void) */ node->setModeOperational(); + // Spin node for device discovery + _node->spin(uavcan::MonotonicDuration::fromMSec(5000)); + + snprintf(_thread_name, sizeof(_thread_name), "uavcan_%u", driver_index); + + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UAVCAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_CAN, 0)) { + _node->setModeOfflineAndPublish(); + debug_uavcan(1, "UAVCAN: couldn't create thread\n\r"); + return; + } + _initialized = true; - - debug_uavcan(1, "UAVCAN: init done\n\r"); - - return true; + debug_uavcan(2, "UAVCAN: init done\n\r"); } void AP_UAVCAN::SRV_sem_take() @@ -584,7 +584,7 @@ void AP_UAVCAN::SRV_send_servos(void) uavcan::equipment::actuator::ArrayCommand msg; SRV_sem_take(); - + uint8_t i; // UAVCAN can hold maximum of 15 commands in one frame for (i = 0; starting_servo < UAVCAN_SRV_NUMBER && i < 15; starting_servo++) { @@ -615,9 +615,9 @@ void AP_UAVCAN::SRV_send_servos(void) } SRV_sem_give(); - + if (i > 0) { - act_out_array[_uavcan_i]->broadcast(msg); + act_out_array[_driver_index]->broadcast(msg); if (i == 15) { repeat_send = true; @@ -649,7 +649,7 @@ void AP_UAVCAN::SRV_send_esc(void) k = 0; SRV_sem_take(); - + for (uint8_t i = 0; i < max_esc_num && k < 20; i++) { if ((((uint32_t) 1) << i) & _esc_bm) { // TODO: ESC negative scaling for reverse thrust and reverse rotation @@ -666,59 +666,59 @@ void AP_UAVCAN::SRV_send_esc(void) } SRV_sem_give(); - esc_raw[_uavcan_i]->broadcast(esc_msg); + esc_raw[_driver_index]->broadcast(esc_msg); } } -void AP_UAVCAN::do_cyclic(void) +void AP_UAVCAN::loop(void) { - if (!_initialized) { - hal.scheduler->delay_microseconds(1000); - return; - } + while (true) { + if (!_initialized) { + hal.scheduler->delay_microseconds(1000); + continue; + } - 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) { - hal.scheduler->delay_microseconds(100); - return; - } + if (error < 0) { + hal.scheduler->delay_microseconds(100); + continue; + } - if (_SRV_armed) { - bool sent_servos = false; - - if (_servo_bm > 0) { - // if we have any Servos in bitmask - uint32_t now = AP_HAL::micros(); - const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get()); - if (now - _SRV_last_send_us >= servo_period_us) { - _SRV_last_send_us = now; - SRV_send_servos(); - sent_servos = true; - for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { - _SRV_conf[i].servo_pending = false; + if (_SRV_armed) { + bool sent_servos = false; + + if (_servo_bm > 0) { + // if we have any Servos in bitmask + uint32_t now = AP_HAL::micros(); + const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get()); + if (now - _SRV_last_send_us >= servo_period_us) { + _SRV_last_send_us = now; + SRV_send_servos(); + sent_servos = true; + for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { + _SRV_conf[i].servo_pending = false; + } } } + + // if we have any ESC's in bitmask + if (_esc_bm > 0 && !sent_servos) { + SRV_send_esc(); + } + + for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { + _SRV_conf[i].esc_pending = false; + } } - // if we have any ESC's in bitmask - if (_esc_bm > 0 && !sent_servos) { - SRV_send_esc(); - } - - for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { - _SRV_conf[i].esc_pending = false; + if (led_out_sem_take()) { + led_out_send(); + led_out_sem_give(); } } - - if (led_out_sem_take()) { - - led_out_send(); - led_out_sem_give(); - } - } bool AP_UAVCAN::led_out_sem_take() @@ -749,7 +749,7 @@ void AP_UAVCAN::led_out_send() } } - rgb_led[_uavcan_i]->broadcast(msg); + rgb_led[_driver_index]->broadcast(msg); _led_conf.last_update = AP_HAL::micros64(); } } @@ -761,11 +761,11 @@ uavcan::ISystemClock & AP_UAVCAN::get_system_clock() uavcan::ICanDriver * AP_UAVCAN::get_can_driver() { - if (_parent_can_mgr != nullptr) { - if (_parent_can_mgr->is_initialized() == false) { + if (hal.can_mgr[_driver_index] != nullptr) { + if (hal.can_mgr[_driver_index]->is_initialized() == false) { return nullptr; } else { - return _parent_can_mgr->get_driver(); + return hal.can_mgr[_driver_index]->get_driver(); } } return nullptr; @@ -1417,12 +1417,13 @@ bool AP_UAVCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t return true; } -AP_UAVCAN *AP_UAVCAN::get_uavcan(uint8_t iface) +AP_UAVCAN *AP_UAVCAN::get_uavcan(uint8_t driver_index) { - if (iface >= MAX_NUMBER_OF_CAN_INTERFACES || !hal.can_mgr[iface]) { + if (driver_index >= AP::can().get_num_drivers() || + AP::can().get_protocol_type(driver_index) != AP_BoardConfig_CAN::Protocol_Type_UAVCAN) { return nullptr; } - return hal.can_mgr[iface]->get_UAVCAN(); + return static_cast(AP::can().get_driver(driver_index)); } #endif // HAL_WITH_UAVCAN diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 985d98c6ed..384ca88a72 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -47,7 +47,7 @@ #define AP_UAVCAN_MAX_LED_DEVICES 4 #define AP_UAVCAN_LED_DELAY_MILLISECONDS 50 -class AP_UAVCAN { +class AP_UAVCAN : public AP_HAL::CANProtocol { public: AP_UAVCAN(); ~AP_UAVCAN(); @@ -258,13 +258,13 @@ private: AP_Int32 _esc_bm; AP_Int16 _servo_rate_hz; - uint8_t _uavcan_i; + uint8_t _driver_index; + char _thread_name[9]; - AP_HAL::CANManager* _parent_can_mgr; + void loop(void); public: - void do_cyclic(void); - bool try_init(void); + void init(uint8_t driver_index) override; void SRV_set_safety_pwm(uint32_t chmask, uint16_t pulse_len); void SRV_set_failsafe_pwm(uint32_t chmask, uint16_t pulse_len); @@ -274,11 +274,6 @@ public: void SRV_write(uint16_t pulse_len, uint8_t ch); void SRV_push_servos(void); bool led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue); - - void set_parent_can_mgr(AP_HAL::CANManager* parent_can_mgr) - { - _parent_can_mgr = parent_can_mgr; - } }; #endif /* AP_UAVCAN_H_ */ diff --git a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp b/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp index 8332520d11..da6860cdb4 100644 --- a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp +++ b/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp @@ -6,14 +6,15 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && HAL_WITH_UAVCAN -#include - #include #include #include +#include + +#include + #include -#include #include #include @@ -42,7 +43,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); #define UAVCAN_NODE_POOL_SIZE 8192 #define UAVCAN_NODE_POOL_BLOCK_SIZE 256 -#define debug_uavcan(level, fmt, args...) do { hal.console->printf(fmt, ##args); } while (0) +#define debug_uavcan(fmt, args...) do { hal.console->printf(fmt, ##args); } while (0) class UAVCAN_sniffer { public: @@ -52,9 +53,9 @@ public: void init(void); void loop(void); void print_stats(void); - + private: - AP_HAL::Semaphore *_led_out_sem; + uint8_t driver_index = 0; class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable { SystemClock() @@ -94,8 +95,6 @@ private: uavcan::Node<0> *_node; uavcan::ISystemClock& get_system_clock(); - uavcan::ICanDriver* get_can_driver(); - uavcan::Node<0>* get_node(); // This will be needed to implement if UAVCAN is used with multithreading // Such cases will be firmware update, etc. @@ -111,13 +110,6 @@ private: }; uavcan::HeapBasedPoolAllocator _node_allocator; - - AP_HAL::CANManager* _parent_can_mgr; - - void set_parent_can_mgr(AP_HAL::CANManager* parent_can_mgr) - { - _parent_can_mgr = parent_can_mgr; - } }; static struct { @@ -145,92 +137,97 @@ static void count_msg(const char *name) MSG_CB(uavcan::protocol::NodeStatus, NodeStatus) MSG_CB(uavcan::equipment::gnss::Fix, Fix) +MSG_CB(uavcan::equipment::gnss::Auxiliary, Auxiliary) MSG_CB(uavcan::equipment::ahrs::MagneticFieldStrength, MagneticFieldStrength) +MSG_CB(uavcan::equipment::ahrs::MagneticFieldStrength2, MagneticFieldStrength2); MSG_CB(uavcan::equipment::air_data::StaticPressure, StaticPressure) MSG_CB(uavcan::equipment::air_data::StaticTemperature, StaticTemperature) -MSG_CB(uavcan::equipment::gnss::Auxiliary, Auxiliary) +MSG_CB(uavcan::equipment::power::BatteryInfo, BatteryInfo); MSG_CB(uavcan::equipment::actuator::ArrayCommand, ArrayCommand) MSG_CB(uavcan::equipment::esc::RawCommand, RawCommand) +MSG_CB(uavcan::equipment::indication::LightsCommand, LightsCommand); void UAVCAN_sniffer::init(void) { - uint8_t inum = 0; - const_cast (hal).can_mgr[inum] = new ChibiOS::CANManager; - hal.can_mgr[0]->begin(1000000, inum); - - set_parent_can_mgr(hal.can_mgr[inum]); + uint8_t interface = 0; + AP_HAL::CANManager* can_mgr = new ChibiOS::CANManager; - if (!_parent_can_mgr->is_initialized()) { - hal.console->printf("Can not initialised\n"); + if (can_mgr == nullptr) { + AP_HAL::panic("Couldn't allocate CANManager, something is very wrong"); + } + + const_cast (hal).can_mgr[driver_index] = can_mgr; + can_mgr->begin(1000000, interface); + can_mgr->initialized(true); + + if (!can_mgr->is_initialized()) { + debug_uavcan("Can not initialised\n"); return; } - auto *node = get_node(); + uavcan::ICanDriver* driver = can_mgr->get_driver(); + if (driver == nullptr) { + return; + } + + _node = new uavcan::Node<0>(*driver, get_system_clock(), _node_allocator); + + if (_node == nullptr) { + return; + } + + if (_node->isStarted()) { + return; + } uavcan::NodeID self_node_id(9); - node->setNodeID(self_node_id); - + _node->setNodeID(self_node_id); + char ndname[20]; - snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", inum); - + snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index); + uavcan::NodeStatusProvider::NodeName name(ndname); - node->setName(name); - + _node->setName(name); + uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion sw_version.major = AP_UAVCAN_SW_VERS_MAJOR; sw_version.minor = AP_UAVCAN_SW_VERS_MINOR; - node->setSoftwareVersion(sw_version); - + _node->setSoftwareVersion(sw_version); + uavcan::protocol::HardwareVersion hw_version; // Standard type uavcan.protocol.HardwareVersion - + hw_version.major = AP_UAVCAN_HW_VERS_MAJOR; hw_version.minor = AP_UAVCAN_HW_VERS_MINOR; - node->setHardwareVersion(hw_version); - - const int node_start_res = node->start(); - if (node_start_res < 0) { - debug_uavcan(1, "UAVCAN: node start problem\n\r"); + _node->setHardwareVersion(hw_version); + + int start_res = _node->start(); + if (start_res < 0) { + debug_uavcan("UAVCAN: node start problem\n\r"); + return; } -#define START_CB(mtype, cbname) (new uavcan::Subscriber(*node))->start(cb_ ## cbname) - +#define START_CB(mtype, cbname) (new uavcan::Subscriber(*_node))->start(cb_ ## cbname) + START_CB(uavcan::protocol::NodeStatus, NodeStatus); START_CB(uavcan::equipment::gnss::Fix, Fix); + START_CB(uavcan::equipment::gnss::Auxiliary, Auxiliary); START_CB(uavcan::equipment::ahrs::MagneticFieldStrength, MagneticFieldStrength); + START_CB(uavcan::equipment::ahrs::MagneticFieldStrength2, MagneticFieldStrength2); START_CB(uavcan::equipment::air_data::StaticPressure, StaticPressure); START_CB(uavcan::equipment::air_data::StaticTemperature, StaticTemperature); - START_CB(uavcan::equipment::gnss::Auxiliary, Auxiliary); + START_CB(uavcan::equipment::power::BatteryInfo, BatteryInfo); START_CB(uavcan::equipment::actuator::ArrayCommand, ArrayCommand); START_CB(uavcan::equipment::esc::RawCommand, RawCommand); - + START_CB(uavcan::equipment::indication::LightsCommand, LightsCommand); + + /* * Informing other nodes that we're ready to work. * Default mode is INITIALIZING. */ - node->setModeOperational(); - - debug_uavcan(1, "UAVCAN: init done\n\r"); -} + _node->setModeOperational(); -uavcan::Node<0> *UAVCAN_sniffer::get_node() -{ - if (_node == nullptr && get_can_driver() != nullptr) { - _node = new uavcan::Node<0>(*get_can_driver(), get_system_clock(), _node_allocator); - } - - return _node; -} - -uavcan::ICanDriver * UAVCAN_sniffer::get_can_driver() -{ - if (_parent_can_mgr != nullptr) { - if (_parent_can_mgr->is_initialized() == false) { - return nullptr; - } else { - return _parent_can_mgr->get_driver(); - } - } - return nullptr; + debug_uavcan("UAVCAN: init done\n\r"); } uavcan::ISystemClock & UAVCAN_sniffer::get_system_clock() @@ -240,12 +237,16 @@ uavcan::ISystemClock & UAVCAN_sniffer::get_system_clock() void UAVCAN_sniffer::loop(void) { - auto *node = get_node(); - node->spin(uavcan::MonotonicDuration::fromMSec(1)); + if (_node == nullptr) { + return; + } + + _node->spin(uavcan::MonotonicDuration::fromMSec(1)); } void UAVCAN_sniffer::print_stats(void) { + hal.console->printf("%lu\n", AP_HAL::micros()); for (uint16_t i=0;i<100;i++) { if (counters[i].msg_name == nullptr) { break; @@ -271,7 +272,6 @@ void setup(void) hal.scheduler->delay(2000); hal.console->printf("Starting UAVCAN sniffer\n"); sniffer.init(); - } void loop(void)