fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

UAVCANv1 cleanup and uORB over UAVCANV1 move to own subclass

Browse Source
This commit is contained in:
Peter van der Perk 2021-05-07 17:54:39 +02:00 committed by Lorenz Meier
parent dfb4ec56b1
commit 0c926250a2
16 changed files with 361 additions and 168 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig
View File

@ -63,7 +63,7 @@ CONFIG_HAVE_CXXINITIALIZE=y
CONFIG_I2C_RESET=y
CONFIG_IDLETHREAD_STACKSIZE=750
CONFIG_IOB_NBUFFERS=48
CONFIG_IOB_NCHAINS=16
CONFIG_IOB_NCHAINS=18
CONFIG_KINETIS_ADC0=y
CONFIG_KINETIS_ADC1=y
CONFIG_KINETIS_CRC=y

2
src/drivers/uavcan_v1/CanardNuttXCDev.cpp
View File

@ -126,7 +126,7 @@ int16_t CanardNuttXCDev::receive(CanardFrame *received_frame)
// Any recieved CAN messages will cause the poll statement to unblock and run
// This way CAN read runs with minimal latency.
// Note that multiple messages may be received in a short time, so this will try to read any availible in a loop
::poll(&fds, 1, 10);
::poll(&fds, 1, 0);
// Only execute this part if can0 is changed.
if (fds.revents & POLLIN) {

4
src/drivers/uavcan_v1/CanardSocketCAN.cpp
View File

@ -162,12 +162,12 @@ int16_t CanardSocketCAN::transmit(const CanardFrame &txf, int timeout_ms)
_send_tv->tv_usec = txf.timestamp_usec % 1000000ULL;
_send_tv->tv_sec = (txf.timestamp_usec - _send_tv->tv_usec) / 1000000ULL;
return sendmsg(_fd, &_send_msg, 1000);
return sendmsg(_fd, &_send_msg, 0);
}
int16_t CanardSocketCAN::receive(CanardFrame *rxf)
{
int32_t result = recvmsg(_fd, &_recv_msg, 1000);
int32_t result = recvmsg(_fd, &_recv_msg, MSG_DONTWAIT);
if (result < 0) {
return result;

28
src/drivers/uavcan_v1/NodeManager.cpp
View File

@ -39,16 +39,20 @@
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#define RETRY_COUNT 10
#include "NodeManager.hpp"
bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
{
if (msg.allocated_node_id.count == 0) {
uint32_t i;
msg.allocated_node_id.count = 1;
msg.allocated_node_id.elements[0].value = CANARD_NODE_ID_UNSET;
/* Search for an available NodeID to assign */
for (uint32_t i = 1; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
for (i = 1; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
if (i == _canard_instance.node_id) {
continue; // Don't give our NodeID to a node
@ -63,6 +67,10 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
}
}
nodeid_registry[i].register_setup = false; // Re-instantiate register setup
nodeid_registry[i].register_index = 0;
nodeid_registry[i].retry_count = 0;
if (msg.allocated_node_id.elements[0].value != CANARD_NODE_ID_UNSET) {
PX4_INFO("Received NodeID allocation request assigning %i", msg.allocated_node_id.elements[0].value);
@ -120,17 +128,15 @@ void NodeManager::HandleListResponse(CanardNodeID node_id, uavcan_register_List_
if (nodeid_registry[i].node_id == node_id) {
nodeid_registry[i].register_index++; // Increment index counter for next update()
nodeid_registry[i].register_setup = false;
nodeid_registry[i].retry_count = 0;
}
}
if (_access_request.setPortId(node_id, msg.name)) {
PX4_INFO("Set portID succesfull");
if (strncmp((char *)msg.name.name.elements, gps_uorb_register_name,
msg.name.name.count) == 0) {
_access_request.setPortId(node_id, gps_uorb_register_name, 1235); //TODO configurable and combine with ParamManager.
} else if (strncmp((char *)msg.name.name.elements, bms_status_register_name,
msg.name.name.count) == 0) { //Battery status publisher
_access_request.setPortId(node_id, bms_status_register_name, 1234); //TODO configurable and combine with ParamManager.
} else {
PX4_INFO("Register not found %.*s", msg.name.name.count, msg.name.name.elements);
}
}
}
@ -142,7 +148,11 @@ void NodeManager::update()
if (nodeid_registry[i].node_id != 0 && nodeid_registry[i].register_setup == false) {
//Setting up registers
_list_request.request(nodeid_registry[i].node_id, nodeid_registry[i].register_index);
nodeid_registry[i].register_setup = true;
nodeid_registry[i].retry_count++;
if (nodeid_registry[i].retry_count > RETRY_COUNT) {
nodeid_registry[i].register_setup = true; // Don't update anymore
}
}
}

8
src/drivers/uavcan_v1/NodeManager.hpp
View File

@ -63,12 +63,14 @@ typedef struct {
uint8_t unique_id[16];
bool register_setup;
uint16_t register_index;
uint16_t retry_count;
} UavcanNodeEntry;
class NodeManager
{
public:
NodeManager(CanardInstance &ins) : _canard_instance(ins), _access_request(ins), _list_request(ins) { };
NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _access_request(ins, pmgr),
_list_request(ins) { };
bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg);
bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg);
@ -89,8 +91,4 @@ private:
bool nodeRegisterSetup = 0;
hrt_abstime _register_request_last{0};
//TODO work this out
const char *gps_uorb_register_name = "uavcan.pub.gnss_uorb.id";
const char *bms_status_register_name = "uavcan.pub.battery_status.id";
};

29
src/drivers/uavcan_v1/ParamManager.cpp
View File

@ -40,6 +40,7 @@
*/
#include "ParamManager.hpp"
#include <px4_platform_common/defines.h>
bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_Value_1_0 &value)
{
@ -56,8 +57,16 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_
case PARAM_TYPE_INT32: {
int32_t out_val {};
param_get(param_handle, &out_val);
value.integer32.value.elements[0] = out_val;
uavcan_register_Value_1_0_select_integer32_(&value);
if (uavcan_register_Value_1_0_is_natural16_(&value)) { //FIXME param rewrite
value.natural16.value.elements[0] = (uint16_t)out_val;
uavcan_register_Value_1_0_select_natural16_(&value);
} else {
value.integer32.value.elements[0] = out_val;
uavcan_register_Value_1_0_select_integer32_(&value);
}
break;
}
@ -91,15 +100,23 @@ bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, ua
switch (param_type(param_handle)) {
case PARAM_TYPE_INT32: {
int32_t out_val {};
param_set(param_handle, &out_val);
value.integer32.value.elements[0] = out_val;
uavcan_register_Value_1_0_select_integer32_(&value);
param_get(param_handle, &out_val);
if (uavcan_register_Value_1_0_is_natural16_(&value)) { //FIXME param rewrite
value.natural16.value.elements[0] = (uint16_t)out_val;
uavcan_register_Value_1_0_select_natural16_(&value);
} else {
value.integer32.value.elements[0] = out_val;
uavcan_register_Value_1_0_select_integer32_(&value);
}
break;
}
case PARAM_TYPE_FLOAT: {
float out_val {};
param_set(param_handle, &out_val);
param_get(param_handle, &out_val);
value.real32.value.elements[0] = out_val;
uavcan_register_Value_1_0_select_real32_(&value);
break;

3
src/drivers/uavcan_v1/ParamManager.hpp
View File

@ -63,7 +63,7 @@ public:
private:
const UavcanParamBinder _uavcan_params[10] {
const UavcanParamBinder _uavcan_params[11] {
{"uavcan.pub.esc.0.id", "UCAN1_ESC_PUB"},
{"uavcan.pub.servo.0.id", "UCAN1_SERVO_PUB"},
{"uavcan.pub.gps.0.id", "UCAN1_GPS_PUB"},
@ -74,6 +74,7 @@ private:
{"uavcan.sub.battery_status.0.id", "UCAN1_BMS_BS_PID"},
{"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_PID"},
{"uavcan.sub.legacy_bms.0.id", "UCAN1_LG_BMS_PID"},
{"uavcan.sub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS"},
//{"uavcan.sub.bms.0.id", "UCAN1_BMS0_PID"}, //FIXME instancing
//{"uavcan.sub.bms.1.id", "UCAN1_BMS1_PID"},
};

84
src/drivers/uavcan_v1/Publishers/uORB/sensor_gps.hpp Normal file
View File

@ -0,0 +1,84 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file sensor_gps.hpp
*
* Defines uORB over UAVCANv1 sensor_gps publisher
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#pragma once
#include <uORB/topics/sensor_gps.h>
#include "../Publisher.hpp"
class UORB_over_UAVCAN_sensor_gps_Publisher : public UavcanPublisher
{
public:
UORB_over_UAVCAN_sensor_gps_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanPublisher(ins, pmgr, "sensor_gps", instance)
{};
// Update the uORB Subscription and broadcast a UAVCAN message
virtual void update() override
{
// Not sure if actuator_armed is a good indication of readiness but seems close to it
if (_sensor_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET && _port_id != 0) {
sensor_gps_s gps_msg {};
_sensor_gps_sub.update(&gps_msg);
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = _port_id, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET,
.transfer_id = _transfer_id,
.payload_size = sizeof(struct sensor_gps_s),
.payload = &gps_msg,
};
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
}
}
};
private:
uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)};
};

58
src/drivers/uavcan_v1/Services/AccessRequest.hpp
View File

@ -52,45 +52,55 @@
class UavcanAccessServiceRequest
{
public:
UavcanAccessServiceRequest(CanardInstance &ins) :
_canard_instance(ins) { };
UavcanAccessServiceRequest(CanardInstance &ins, UavcanParamManager &pmgr) :
_canard_instance(ins), _param_manager(pmgr) { };
void setPortId(CanardNodeID node_id, const char *register_name, uint16_t port_id)
bool setPortId(CanardNodeID node_id, uavcan_register_Name_1_0 &name)
{
int result {0};
uavcan_register_Access_Request_1_0 request_msg;
strncpy((char *)&request_msg.name.name.elements[0], register_name, sizeof(uavcan_register_Name_1_0));
request_msg.name.name.count = strlen(register_name);
uavcan_register_Value_1_0_select_natural16_(&request_msg.value);
request_msg.value.natural16.value.count = 1;
request_msg.value.natural16.value.elements[0] = port_id;
uavcan_register_Value_1_0_select_natural16_(&request_msg.value); // Set to natural16 so that ParamManager casts type
uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
//FIXME ParamManager only has notion of being either sub/pub have to find a portable way to address trhis
name.name.elements[7] = 's'; //HACK Change pub into sub
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindRequest,
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = node_id, // Messages cannot be unicast, so use UNSET.
.transfer_id = access_request_transfer_id,
.payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &request_payload_buffer,
};
if (_param_manager.GetParamByName(name, request_msg.value)) {
name.name.elements[7] = 'p'; //HACK Change sub into pub
memcpy(&request_msg.name, &name, sizeof(request_msg.name));
result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
if (result == 0) {
// set the data ready in the buffer and chop if needed
++access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindRequest,
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = node_id, // Messages cannot be unicast, so use UNSET.
.transfer_id = access_request_transfer_id,
.payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &request_payload_buffer,
};
result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
}
return result > 0;
} else {
return false;
}
};
private:
CanardInstance &_canard_instance;
CanardTransferID access_request_transfer_id = 0;
UavcanParamManager &_param_manager;
};

32
src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp
View File

@ -68,25 +68,27 @@ public:
// Set _port_id from _uavcan_param
uavcan_register_Value_1_0 value;
_param_manager.GetParamByName(uavcan_param, value);
int32_t new_id = value.integer32.value.elements[0];
/* FIXME how about partial subscribing */
if (curSubj->_canard_sub._port_id != new_id) {
if (new_id == CANARD_PORT_ID_UNSET) {
// Cancel subscription
unsubscribe();
if (_param_manager.GetParamByName(uavcan_param, value)) {
int32_t new_id = value.integer32.value.elements[0];
} else {
if (curSubj->_canard_sub._port_id != CANARD_PORT_ID_UNSET) {
// Already active; unsubscribe first
/* FIXME how about partial subscribing */
if (curSubj->_canard_sub._port_id != new_id) {
if (new_id == CANARD_PORT_ID_UNSET) {
// Cancel subscription
unsubscribe();
}
// Subscribe on the new port ID
curSubj->_canard_sub._port_id = (CanardPortID)new_id;
PX4_INFO("Subscribing %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub._port_id);
subscribe();
} else {
if (curSubj->_canard_sub._port_id != CANARD_PORT_ID_UNSET) {
// Already active; unsubscribe first
unsubscribe();
}
// Subscribe on the new port ID
curSubj->_canard_sub._port_id = (CanardPortID)new_id;
PX4_INFO("Subscribing %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub._port_id);
subscribe();
}
}
}

83
src/drivers/uavcan_v1/Subscribers/uORB/sensor_gps.hpp Normal file
View File

@ -0,0 +1,83 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file sensor_gps.hpp
*
* Defines uORB over UAVCANv1 sensor_gps subscriber
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#pragma once
#include <uORB/topics/sensor_gps.h>
#include <uORB/PublicationMulti.hpp>
#include "../DynamicPortSubscriber.hpp"
class UORB_over_UAVCAN_sensor_gps_Subscriber : public UavcanDynamicPortSubscriber
{
public:
UORB_over_UAVCAN_sensor_gps_Subscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
UavcanDynamicPortSubscriber(ins, pmgr, "uorb.sensor_gps", instance) { };
void subscribe() override
{
// Subscribe to messages uORB sensor_gps payload over UAVCAN
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_subj_sub._canard_sub._port_id,
sizeof(struct sensor_gps_s),
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000,
&_subj_sub._canard_sub);
};
void callback(const CanardTransfer &receive) override
{
//PX4_INFO("uORB sensor_gps Callback");
if (receive.payload_size == sizeof(struct sensor_gps_s)) {
sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload;
_sensor_gps_pub.publish(*gps_msg);
} else {
PX4_ERR("uORB over UAVCAN %s playload size mismatch got %d expected %d",
_subj_sub._subject_name, receive.payload_size, sizeof(struct sensor_gps_s));
}
};
private:
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
};

156
src/drivers/uavcan_v1/Uavcan.cpp
View File

@ -56,8 +56,7 @@ static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree
UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
_can_interface(interface)//,
// _subscribers{&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub}
_can_interface(interface)
{
pthread_mutex_init(&_node_mutex, nullptr);
@ -168,13 +167,6 @@ void UavcanNode::init()
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_heartbeat_subscription);
canardRxSubscribe(&_canard_instance, // uORB over UAVCAN GPS message
CanardTransferKindMessage,
gps_port_id,
sizeof(struct sensor_gps_s),
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_drone_srv_gps_subscription);
for (auto &subscriber : _subscribers) {
subscriber->subscribe();
}
@ -238,7 +230,69 @@ void UavcanNode::Run()
_node_manager.update();
// Transmitting
transmit();
/* Process received messages */
uint8_t data[64] {};
CanardFrame received_frame{};
received_frame.payload = &data;
while (_can_interface->receive(&received_frame) > 0) {
CanardTransfer receive{};
int32_t result = canardRxAccept(&_canard_instance, &received_frame, 0, &receive);
if (result < 0) {
// An error has occurred: either an argument is invalid or we've ran out of memory.
// It is possible to statically prove that an out-of-memory will never occur for a given application if
// the heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
// Reception of an invalid frame is NOT an error.
PX4_ERR("Receive error %d\n", result);
} else if (result == 1) {
// A transfer has been received, process it.
// PX4_INFO("received Port ID: %d", receive.port_id);
if (receive.port_id > 0) {
// If not a fixed port ID, check any subscribers which may have registered it
for (auto &subscriber : _subscribers) {
if (subscriber->hasPortID(receive.port_id)) {
subscriber->callback(receive);
}
}
for (auto &subscriber : _dynsubscribers) {
if (subscriber->hasPortID(receive.port_id)) {
subscriber->callback(receive);
}
}
}
// Deallocate the dynamic memory afterwards.
_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
} else {
//PX4_INFO("RX canard %d", result);
}
}
// Pop canardTx queue to send out responses to requets
transmit();
perf_end(_cycle_perf);
if (_task_should_exit.load()) {
_can_interface->close();
ScheduleClear();
_instance = nullptr;
}
pthread_mutex_unlock(&_node_mutex);
}
void UavcanNode::transmit()
{
// Look at the top of the TX queue.
for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) {
// Attempt transmission only if the frame is not yet timed out while waiting in the TX queue.
@ -268,67 +322,6 @@ void UavcanNode::Run()
}
}
}
/* Process received messages */
uint8_t data[64] {};
CanardFrame received_frame{};
received_frame.payload = &data;
/* FIXME this flawed we've to go through the whole loop to get the next frame in the buffer */
if (_can_interface->receive(&received_frame) > 0) {
CanardTransfer receive{};
int32_t result = canardRxAccept(&_canard_instance, &received_frame, 0, &receive);
if (result < 0) {
// An error has occurred: either an argument is invalid or we've ran out of memory.
// It is possible to statically prove that an out-of-memory will never occur for a given application if
// the heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
// Reception of an invalid frame is NOT an error.
PX4_ERR("Receive error %d\n", result);
} else if (result == 1) {
// A transfer has been received, process it.
// PX4_INFO("received Port ID: %d", receive.port_id);
if (receive.port_id == gps_port_id) {
result = handleUORBSensorGPS(receive);
} else if (receive.port_id > 0) {
// If not a fixed port ID, check any subscribers which may have registered it
for (auto &subscriber : _subscribers) {
if (subscriber->hasPortID(receive.port_id)) {
subscriber->callback(receive);
}
}
for (auto &subscriber : _dynsubscribers) {
if (subscriber->hasPortID(receive.port_id)) {
subscriber->callback(receive);
}
}
}
// Deallocate the dynamic memory afterwards.
_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
} else {
//PX4_INFO("RX canard %d\r\n", result);
}
}
perf_end(_cycle_perf);
if (_task_should_exit.load()) {
_can_interface->close();
ScheduleClear();
_instance = nullptr;
}
pthread_mutex_unlock(&_node_mutex);
}
void UavcanNode::print_info()
@ -338,6 +331,13 @@ void UavcanNode::print_info()
perf_print_counter(_cycle_perf);
perf_print_counter(_interval_perf);
O1HeapDiagnostics heap_diagnostics = o1heapGetDiagnostics(uavcan_allocator);
PX4_INFO("Heap status %zu/%zu Peak alloc %zu Peak req %zu OOM count %" PRIu64,
heap_diagnostics.allocated, heap_diagnostics.capacity,
heap_diagnostics.peak_allocated, heap_diagnostics.peak_request_size,
heap_diagnostics.oom_count);
for (auto &publisher : _publishers) {
publisher->printInfo();
}
@ -346,6 +346,10 @@ void UavcanNode::print_info()
subscriber->printInfo();
}
for (auto &subscriber : _dynsubscribers) {
subscriber->printInfo();
}
_mixing_output.printInfo();
_esc_controller.printInfo();
@ -442,16 +446,6 @@ void UavcanNode::sendHeartbeat()
}
}
int UavcanNode::handleUORBSensorGPS(const CanardTransfer &receive)
{
PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id);
sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload;
return _sensor_gps_pub.publish(*gps_msg) ? 0 : -1;
}
bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{

30
src/drivers/uavcan_v1/Uavcan.hpp
View File

@ -78,6 +78,9 @@
#include "Subscribers/NodeIDAllocationData.hpp"
#include "Subscribers/LegacyBatteryInfo.hpp"
// uORB over UAVCAN subscribers
#include "Subscribers/uORB/sensor_gps.hpp"
#include "ServiceClients/GetInfo.hpp"
#include "ServiceClients/Access.hpp"
@ -162,14 +165,11 @@ private:
void Run() override;
void fill_node_info();
void transmit();
// Sends a heartbeat at 1s intervals
void sendHeartbeat();
int handlePnpNodeIDAllocationData(const CanardTransfer &receive);
int handleRegisterList(const CanardTransfer &receive);
int handleRegisterAccess(const CanardTransfer &receive);
int handleUORBSensorGPS(const CanardTransfer &receive);
void *_uavcan_heap{nullptr};
CanardInterface *const _can_interface;
@ -185,14 +185,9 @@ private:
pthread_mutex_t _node_mutex;
CanardRxSubscription _heartbeat_subscription;
CanardRxSubscription _drone_srv_gps_subscription;
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Publication<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
uORB::Publication<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
@ -201,15 +196,6 @@ private:
hrt_abstime _uavcan_node_heartbeat_last{0};
CanardTransferID _uavcan_node_heartbeat_transfer_id{0};
/* Temporary hardcoded port IDs used by the register interface
* for demo purposes untill we have nice interface (QGC or latter)
* to configure the nodes
*/
const uint16_t gps_port_id = 1235;
CanardTransferID _uavcan_register_list_request_transfer_id{0};
CanardTransferID _uavcan_register_access_request_transfer_id{0};
DEFINE_PARAMETERS(
(ParamInt<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,
(ParamInt<px4::params::UAVCAN_V1_ID>) _param_uavcan_v1_id,
@ -220,7 +206,7 @@ private:
UavcanParamManager _param_manager;
NodeManager _node_manager {_canard_instance};
NodeManager _node_manager {_canard_instance, _param_manager};
UavcanGnssPublisher _gps_pub {_canard_instance, _param_manager};
@ -238,6 +224,8 @@ private:
UavcanLegacyBatteryInfoSubscriber _legacybms_sub {_canard_instance, _param_manager, 0};
UavcanNodeIDAllocationDataSubscriber _nodeid_sub {_canard_instance, _node_manager};
UORB_over_UAVCAN_sensor_gps_Subscriber _sensor_gps_sub {_canard_instance, _param_manager, 0};
UavcanGetInfoResponse _getinfo_rsp {_canard_instance};
UavcanAccessResponse _access_rsp {_canard_instance, _param_manager};
@ -245,7 +233,7 @@ private:
UavcanListServiceReply _list_service {_canard_instance, _node_manager};
// Subscriber objects: Any object used to bridge a UAVCAN message to a uORB message
UavcanDynamicPortSubscriber *_dynsubscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_esc_sub, &_legacybms_sub}; /// TODO: turn into List<UavcanSubscription*>
UavcanDynamicPortSubscriber *_dynsubscribers[6] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_esc_sub, &_legacybms_sub, &_sensor_gps_sub}; /// TODO: turn into List<UavcanSubscription*>
UavcanBaseSubscriber *_subscribers[5] {&_nodeid_sub, &_getinfo_rsp, &_access_rsp, &_access_service, &_list_service}; /// TODO: turn into List<UavcanSubscription*>
UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};

3
src/drivers/uavcan_v1/parameters.c
View File

@ -103,3 +103,6 @@ PARAM_DEFINE_INT32(UCAN1_LG_BMS_PID, 0);
PARAM_DEFINE_INT32(UCAN1_ESC_PUB, 0);
PARAM_DEFINE_INT32(UCAN1_GPS_PUB, 0);
PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, 0);
// uORB over UAVCAN
PARAM_DEFINE_INT32(UCAN1_UORB_GPS, 0);

2
src/drivers/uavcannode_gps_demo/canard_main.c
View File

@ -381,7 +381,7 @@ static int canard_daemon(int argc, char *argv[])
/* Init UAVCAN register interfaces */
uavcan_node_GetInfo_Response_1_0 node_information; // TODO ADD INFO
uavcan_register_interface_init(&ins, &node_information);
uavcan_register_interface_add_entry("gnss_uorb", set_gps_uorb_port_id, get_gps_uorb_port_id);
uavcan_register_interface_add_entry("uorb.sensor_gps.0", set_gps_uorb_port_id, get_gps_uorb_port_id);
uavcan_register_interface_add_entry("gnss_fix", set_gps_fix_port_id, get_gps_fix_port_id);
uavcan_register_interface_add_entry("gnss_aux", set_gps_aux_port_id, get_gps_aux_port_id);

5
src/drivers/uavcannode_gps_demo/libcancl/registerinterface.c
View File

@ -238,10 +238,13 @@ int32_t uavcan_register_interface_list_response(CanardInstance *ins, CanardTrans
// Reponse magic start
if (msg.index <= register_list_size) {
if (msg.index < register_list_size) {
response_msg.name.name.count = sprintf(response_msg.name.name.elements,
"uavcan.pub.%s.id",
register_list[msg.index].name);
} else {
response_msg.name.name.count = 0;
}
//TODO more option then pub (sub rate