mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-05 23:43:58 -04:00
AP_UAVCAN: adapt to new CANProtocol interface
This includes creating own thread Also adapts example
This commit is contained in:
Francisco Ferreira
parent
126f1379d3
commit
8c382b6904
@ -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<uavcan::equipment::gnss::Fix> *gnss_fix;
|
||||
gnss_fix = new uavcan::Subscriber<uavcan::equipment::gnss::Fix>(*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<uavcan::equipment::gnss::Auxiliary> *gnss_aux;
|
||||
gnss_aux = new uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary>(*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<uavcan::equipment::ahrs::MagneticFieldStrength> *magnetic;
|
||||
magnetic = new uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength>(*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<uavcan::equipment::ahrs::MagneticFieldStrength2> *magnetic2;
|
||||
magnetic2 = new uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength2>(*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<uavcan::equipment::air_data::StaticPressure> *air_data_sp;
|
||||
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_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<uavcan::equipment::air_data::StaticTemperature> *air_data_st;
|
||||
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_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<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]);
|
||||
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<uavcan::equipment::actuator::ArrayCommand>(*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<uavcan::equipment::actuator::ArrayCommand>(*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<uavcan::equipment::esc::RawCommand>(*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<uavcan::equipment::esc::RawCommand>(*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<uavcan::equipment::indication::LightsCommand>(*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<uavcan::equipment::indication::LightsCommand>(*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_UAVCAN*>(AP::can().get_driver(driver_index));
|
||||
}
|
||||
|
||||
#endif // HAL_WITH_UAVCAN
|
||||
|
||||
@ -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_ */
|
||||
|
||||
@ -6,14 +6,15 @@
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && HAL_WITH_UAVCAN
|
||||
|
||||
#include <uavcan/uavcan.hpp>
|
||||
|
||||
#include <AP_HAL/CAN.h>
|
||||
#include <AP_HAL/Semaphores.h>
|
||||
#include <AP_HAL_ChibiOS/CAN.h>
|
||||
|
||||
#include <AP_UAVCAN/AP_UAVCAN.h>
|
||||
|
||||
#include <uavcan/uavcan.hpp>
|
||||
|
||||
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
|
||||
#include <uavcan/equipment/indication/RGB565.hpp>
|
||||
|
||||
#include <uavcan/equipment/gnss/Fix.hpp>
|
||||
#include <uavcan/equipment/gnss/Auxiliary.hpp>
|
||||
@ -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<UAVCAN_NODE_POOL_BLOCK_SIZE, UAVCAN_sniffer::RaiiSynchronizer> _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 <AP_HAL::HAL&> (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 <AP_HAL::HAL&> (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<mtype>(*node))->start(cb_ ## cbname)
|
||||
|
||||
#define START_CB(mtype, cbname) (new uavcan::Subscriber<mtype>(*_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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user