Skip to content

changes to support lua DroneCAN messaging #74

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
tridge merged 1 commit into from
May 5, 2025
Merged

changes to support lua DroneCAN messaging #74

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 15 additions & 5 deletions canard/handler_list.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,16 +85,20 @@ class HandlerList {
while (entry != nullptr) {
if (transfer.data_type_id == entry->msgid &&
entry->transfer_type == transfer.transfer_type) {
entry->handle_message(transfer);
return;
Copy link
Collaborator

@tpwrules tpwrules Apr 24, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's unclear to me why we need to be able to call multiple handler list entries in the first place anyway (c.f. first commit in this PR), but I don't think I've chewed on it enough. Theoretically if you created a new Subscriber, then it would be in the branch list of any old Subscriber and get called.

Should the first commit just be dropped? I think the commit to simplify handling in effect reverts it?

Copy link
Member Author

@tridge tridge Apr 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

i've folded that commit into the later commit

if (entry->handle_message(transfer) &&
Copy link
Collaborator

@tpwrules tpwrules Apr 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think the semantics of this return value are not good and don't match the original behavior. I think handle_message should return true if the handler is the final consumer of the message and false if other handlers should get a chance. Then the loop here also does not need to check the transfer type, it is implied by the handler.

Copy link
Member Author

@tridge tridge May 5, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

i changed it so the return value means that the message was for the right recipient

transfer.transfer_type != CanardTransferTypeBroadcast) {
// we only allow one request or response for non-broadcast
break;
}
}
entry = entry->next;
}
}

/// @brief Method to handle a message implemented by the derived class
/// @param transfer transfer object of the request
virtual void handle_message(const CanardRxTransfer& transfer) = 0;
/// @return true if the message is for this consumer
virtual bool handle_message(const CanardRxTransfer& transfer) = 0;

protected:
virtual ~HandlerList() {}
Expand All @@ -104,14 +108,20 @@ class HandlerList {
static Canard::Semaphore sem[CANARD_NUM_HANDLERS];
#endif

// add ourselves to the handler list. the caller must be holding the semaphore.
// add ourselves to the handler list
void link(void) NOINLINE_FUNC {
#ifdef WITH_SEMAPHORE
WITH_SEMAPHORE(sem[index]);
#endif
next = head[index];
head[index] = this;
}

// remove ourselves from the handler list. the caller must be holding the semaphore.
// remove ourselves from the handler list
void unlink(void) NOINLINE_FUNC {
#ifdef WITH_SEMAPHORE
WITH_SEMAPHORE(sem[index]);
#endif
HandlerList* entry = head[index];
if (entry == this) {
head[index] = next;
Expand Down
55 changes: 12 additions & 43 deletions canard/service_client.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,56 +41,30 @@ class Client : public HandlerList, public Sender {
Sender(_interface),
server_node_id(255),
cb(_cb) {
#ifdef WITH_SEMAPHORE
WITH_SEMAPHORE(sem[index]);
#endif
next = branch_head[index];
branch_head[index] = this;
link(); // link ourselves into the handler list now that we're in the branch list
// link ourselves into the handler list
link();
}

// delete copy constructor and assignment operator
Client(const Client&) = delete;

// destructor, remove the entry from the singly-linked list
// destructor
~Client() NOINLINE_FUNC {
#ifdef WITH_SEMAPHORE
WITH_SEMAPHORE(sem[index]);
#endif
unlink(); // unlink ourselves from the handler list before the branch list
Client<rsptype>* entry = branch_head[index];
if (entry == this) {
branch_head[index] = next;
return;
}
while (entry != nullptr) {
if (entry->next == this) {
entry->next = next;
return;
}
entry = entry->next;
}
// unlink ourselves from the handler list
unlink();
}

/// @brief handles incoming messages
/// @param transfer transfer object of the request
void handle_message(const CanardRxTransfer& transfer) override NOINLINE_FUNC {
bool handle_message(const CanardRxTransfer& transfer) override NOINLINE_FUNC {
Copy link
Collaborator

@tpwrules tpwrules Apr 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should return true iff server_node_id and transfer_id match.

rsptype msg {};
if (rsptype::cxx_iface::rsp_decode(&transfer, &msg)) {
// invalid decode
return;
}

// scan through the list of entries for corresponding server node id and transfer id
Client<rsptype>* entry = branch_head[index];
while (entry != nullptr) {
if (entry->server_node_id == transfer.source_node_id
&& entry->transfer_id == transfer.transfer_id) {
entry->cb(transfer, msg);
return;
}
entry = entry->next;
if (server_node_id == transfer.source_node_id &&
transfer_id == transfer.transfer_id &&
!rsptype::cxx_iface::rsp_decode(&transfer, &msg)) {
cb(transfer, msg);
return true;
}
return false;
}

/// @brief makes service request
Expand Down Expand Up @@ -139,16 +113,11 @@ class Client : public HandlerList, public Sender {
}

private:
static Client<rsptype>* branch_head[CANARD_NUM_HANDLERS];
Client<rsptype>* next;
uint8_t server_node_id;

uint8_t req_buf[rsptype::cxx_iface::REQ_MAX_SIZE];
Callback<rsptype> &cb;
uint8_t transfer_id;
};

template <typename rsptype>
Client<rsptype> *Client<rsptype>::branch_head[] = {nullptr};

} // namespace Canard
20 changes: 7 additions & 13 deletions canard/service_server.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,33 +41,27 @@ class Server : public HandlerList {
HandlerList(CanardTransferTypeRequest, reqtype::cxx_iface::ID, reqtype::cxx_iface::SIGNATURE, _interface.get_index()),
interface(_interface),
cb(_cb) {
#ifdef WITH_SEMAPHORE
WITH_SEMAPHORE(sem[index]);
#endif
link(); // link ourselves into the handler list
}

// delete copy constructor and assignment operator
Server(const Server&) = delete;

~Server() NOINLINE_FUNC {
#ifdef WITH_SEMAPHORE
WITH_SEMAPHORE(sem[index]);
#endif
unlink(); // unlink ourselves from the handler list
}

/// @brief handles incoming messages
/// @param transfer transfer object of the request
void handle_message(const CanardRxTransfer& transfer) override NOINLINE_FUNC {
/// @return true if message has been handled
bool handle_message(const CanardRxTransfer& transfer) override NOINLINE_FUNC {
Copy link
Collaborator

@tpwrules tpwrules Apr 25, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should always return true.

reqtype msg {};
if (reqtype::cxx_iface::req_decode(&transfer, &msg)) {
// invalid decode
return;
if (!reqtype::cxx_iface::req_decode(&transfer, &msg)) {
transfer_id = transfer.transfer_id;
cb(transfer, msg);
return true;
}
transfer_id = transfer.transfer_id;
// call the registered callback
cb(transfer, msg);
return false;
}

/// @brief Send a response to the request
Expand Down
46 changes: 9 additions & 37 deletions canard/subscriber.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,62 +41,34 @@ class Subscriber : public HandlerList {
Subscriber(Callback<msgtype> &_cb, uint8_t _index) NOINLINE_FUNC :
HandlerList(CanardTransferTypeBroadcast, msgtype::cxx_iface::ID, msgtype::cxx_iface::SIGNATURE, _index),
cb (_cb) {
#ifdef WITH_SEMAPHORE
WITH_SEMAPHORE(sem[index]);
#endif
next = branch_head[index];
branch_head[index] = this;
link(); // link ourselves into the handler list now that we're in the branch list
// link ourselves into the handler list
link();
}

// delete copy constructor and assignment operator
Subscriber(const Subscriber&) = delete;

// destructor, remove the entry from the singly-linked list
~Subscriber() NOINLINE_FUNC {
#ifdef WITH_SEMAPHORE
WITH_SEMAPHORE(sem[index]);
#endif
unlink(); // unlink ourselves from the handler list before the branch list
Subscriber<msgtype>* entry = branch_head[index];
if (entry == this) {
branch_head[index] = next;
return;
}
while (entry != nullptr) {
if (entry->next == this) {
entry->next = next;
return;
}
entry = entry->next;
}
// unlink ourselves from the handler list
unlink();
}

/// @brief parse the message and call the callback
/// @param transfer transfer object
void handle_message(const CanardRxTransfer& transfer) override NOINLINE_FUNC {
bool handle_message(const CanardRxTransfer& transfer) override NOINLINE_FUNC {
Copy link
Collaborator

@tpwrules tpwrules Apr 25, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should return true iff decode fails (avoid wasting time on future failing decodes).

Copy link
Member Author

@tridge tridge May 5, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

not quite, one of the advantages of lua DroneCAN is we can fix incorrect DSDL. We have had that from vendors, where DSDL that we don't control (eg. Hobbywing) changes. If we try to short circuit on decode failure then a lua receiver can't fix the issue

Copy link
Collaborator

@tpwrules tpwrules May 5, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Okay fair enough, then it should always return false.

msgtype msg {};
if (msgtype::cxx_iface::decode(&transfer, &msg)) {
// invalid decode
return;
}
// call all registered callbacks in one go
Subscriber<msgtype>* entry = branch_head[index];
while (entry != nullptr) {
entry->cb(transfer, msg);
entry = entry->next;
if (!msgtype::cxx_iface::decode(&transfer, &msg)) {
cb(transfer, msg);
return true;
}
return false;
}

private:
Subscriber<msgtype>* next;
static Subscriber<msgtype> *branch_head[CANARD_NUM_HANDLERS];
Comment on lines -92 to -93
Copy link
Collaborator

@tpwrules tpwrules Apr 24, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I like the idea of getting rid of these, it saves a lot of complexity and memory for something that I don't think was really ever used. However, I am concerned about the performance as handling each message will now have to search through the full list of handlers, instead of only the (on average) half until it gets to its desired handler.

Could we make link() do a sort of sorted insert and keep the same types together? If there are none existing, then just put it at the front to preserve existing behavior. Then the handler loop can early exit after it finds all handlers of the current type.

Copy link
Member Author

@tridge tridge Apr 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

i think we could make it an expanding array possibly with sort on insert and bisection search in the future, but I don't think it is needed for this PR.
having more predictable performance cost is in some ways a benefit. It was essentially random before

Copy link
Collaborator

@tpwrules tpwrules Apr 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I see your point about the randomness, but I think a 100% overhead is at least worth benchmarking. Note also that this means we pay the decode cost multiple times, which can be high.

Copy link
Member Author

@tridge tridge Apr 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

we don't pay the decode cost multiple times unless we actually have multiple subscribers to the message, which we don't unless a lua script subscribes to a message that ardupilot also subscribes to. That will not be common, and when it does happen we don't have much of a choice as we can't instantiate the templated type at runtime in lua based on variables for the data type in the script

Callback<msgtype> &cb;
};

template <typename msgtype>
Subscriber<msgtype>* Subscriber<msgtype>::branch_head[] = {nullptr};

template <typename T, typename msgtype>
class SubscriberArgCb {
public:
Expand Down