AP_UAVCAN::Mag_Info *AP_UAVCAN::find_mag_node(uint8_t node, uint8_t sensor_id)
{
// Check if such node is already defined
for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) {
if (_mag_nodes[i] == node) {
if (_mag_node_max_sensorid_count[i] < sensor_id) {
_mag_node_max_sensorid_count[i] = sensor_id;
debug_uavcan(2, "AP_UAVCAN: Compass: found sensor id %d on node %d\n\r", (int)(sensor_id), (int)(node));
}
return &_mag_node_state[i];
}
}
// If not - try to find free space for it
for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) {
if (_mag_nodes[i] == UINT8_MAX) {
_mag_nodes[i] = node;
_mag_node_max_sensorid_count[i] = (sensor_id ? sensor_id : 1);
debug_uavcan(2, "AP_UAVCAN: Compass: register sensor id %d on node %d\n\r", (int)(sensor_id), (int)(node));
return &_mag_node_state[i];
}
}
// If no space is left - return nullptr
return nullptr;
}
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");
}
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::update_mag_state(uint8_t node, uint8_t sensor_id)
{
// Go through all listeners of specified node and call their's update methods
for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) {
if (_mag_nodes[i] == node) {
for (uint8_t j = 0; j < AP_UAVCAN_MAX_LISTENERS; j++) {
if (_mag_listener_to_node[j] == i) {
/*If the current listener has default sensor_id,
while our sensor_id is not default, we have
to assign our sensor_id to this listener*/
if ((_mag_listener_sensor_ids[j] == 0) && (sensor_id != 0)) {
bool already_taken = false;
for (uint8_t k = 0; k < AP_UAVCAN_MAX_LISTENERS; k++) {
if (_mag_listener_sensor_ids[k] == sensor_id) {
already_taken = true;
}
}
if (!already_taken) {
debug_uavcan(2, "AP_UAVCAN: Compass: sensor_id updated to %d for listener %d\n", sensor_id, j);
_mag_listener_sensor_ids[j] = sensor_id;
}
}
/*If the current listener has the sensor_id that we have,
or our sensor_id is default, ask the listener to handle the measurements
(the default one is used for the nodes that have only one compass*/
if ((sensor_id == 0) || (_mag_listener_sensor_ids[j] == sensor_id)) {
_mag_listeners[j]->handle_mag_msg(_mag_node_state[i].mag_vector);
}
}
}
}
}
}
bool AP_UAVCAN::rc_out_sem_take()
{
bool sem_ret = _rc_out_sem->take(10);
if (!sem_ret) {
debug_uavcan(1, "AP_UAVCAN RCOut semaphore fail\n\r");
}
return sem_ret;
}
uint8_t AP_UAVCAN::register_baro_listener(AP_Baro_Backend* new_listener, uint8_t preferred_channel)
{
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) {
if (preferred_channel != 0) {
if (preferred_channel < AP_UAVCAN_MAX_BARO_NODES) {
_baro_listeners[sel_place] = new_listener;
_baro_listener_to_node[sel_place] = preferred_channel - 1;
_baro_node_taken[_baro_listener_to_node[sel_place]]++;
ret = preferred_channel;
debug_uavcan(2, "reg_Baro place:%d, chan: %d\n\r", sel_place, preferred_channel);
}
} else {
for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) {
if (_baro_node_taken[i] == 0) {
_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;
}
AP_UAVCAN::AP_UAVCAN() :
_node_allocator(
UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_SIZE)
{
AP_Param::setup_object_defaults(this, var_info);
for (uint8_t i = 0; i < UAVCAN_RCO_NUMBER; i++) {
_rco_conf[i].active = false;
}
for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) {
_gps_nodes[i] = UINT8_MAX;
_gps_node_taken[i] = 0;
}
for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) {
_baro_nodes[i] = UINT8_MAX;
_baro_node_taken[i] = 0;
}
for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) {
_mag_nodes[i] = UINT8_MAX;
_mag_node_taken[i] = 0;
_mag_node_max_sensorid_count[i] = 1;
}
for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) {
_gps_listener_to_node[i] = UINT8_MAX;
_gps_listeners[i] = nullptr;
_baro_listener_to_node[i] = UINT8_MAX;
_baro_listeners[i] = nullptr;
_mag_listener_to_node[i] = UINT8_MAX;
_mag_listeners[i] = nullptr;
_mag_listener_sensor_ids[i] = 0;
}
_rc_out_sem = hal.util->new_semaphore();
debug_uavcan(2, "AP_UAVCAN constructed\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;
}
