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]);
if (!_parent_can_mgr->is_initialized()) {
hal.console->printf("Can not initialised\n");
return;
}
auto *node = get_node();
uavcan::NodeID self_node_id(9);
node->setNodeID(self_node_id);
char ndname[20];
snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", inum);
uavcan::NodeStatusProvider::NodeName name(ndname);
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);
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");
}
#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::ahrs::MagneticFieldStrength, MagneticFieldStrength);
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::actuator::ArrayCommand, ArrayCommand);
START_CB(uavcan::equipment::esc::RawCommand, RawCommand);
/*
* Informing other nodes that we're ready to work.
* Default mode is INITIALIZING.
*/
node->setModeOperational();
debug_uavcan(1, "UAVCAN: init done\n\r");
}
bool AP_UAVCAN::try_init(void)
{
if (_parent_can_mgr != nullptr) {
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();
if (node != nullptr) {
if (!node->isStarted()) {
uavcan::NodeID self_node_id(_uavcan_node);
node->setNodeID(self_node_id);
char ndname[20];
snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", _uavcan_i);
uavcan::NodeStatusProvider::NodeName name(ndname);
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);
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");
}
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]);
if (gnss_fix_start_res < 0) {
debug_uavcan(1, "UAVCAN GNSS subscriber start problem\n\r");
return false;
}
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]);
if (gnss_aux_start_res < 0) {
debug_uavcan(1, "UAVCAN GNSS Aux subscriber start problem\n\r");
return false;
}
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]);
if (magnetic_start_res < 0) {
debug_uavcan(1, "UAVCAN Compass subscriber start problem\n\r");
return false;
}
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]);
if (air_data_sp_start_res < 0) {
debug_uavcan(1, "UAVCAN Baro subscriber start problem\n\r");
return false;
}
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]);
if (air_data_st_start_res < 0) {
debug_uavcan(1, "UAVCAN Temperature 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);
esc_raw[_uavcan_i] = new uavcan::Publisher<uavcan::equipment::esc::RawCommand>(*node);
esc_raw[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
esc_raw[_uavcan_i]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
/*
* Informing other nodes that we're ready to work.
* Default mode is INITIALIZING.
*/
node->setModeOperational();
_initialized = true;
debug_uavcan(1, "UAVCAN: init done\n\r");
return true;
}
}
}
if (_initialized) {
return true;
}
}
return false;
}
