aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_OpenDroneID: send dronecan messages directly from update

Browse Source
This commit is contained in:
bugobliterator 2023-05-24 16:03:35 +10:00 committed by Andrew Tridgell
parent cbef055f6b
commit 4b3ae60964
3 changed files with 17 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_OpenDroneID/AP_OpenDroneID.cpp
View File

@ -38,6 +38,7 @@
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Parachute/AP_Parachute.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_DroneCAN/AP_DroneCAN.h>
extern const AP_HAL::HAL &hal;
@ -156,6 +157,20 @@ void AP_OpenDroneID::update()
send_dynamic_out();
send_static_out();
#if HAL_ENABLE_DRONECAN_DRIVERS
uint8_t can_num_drivers = AP::can().get_num_drivers();
for (uint8_t i = 0; i < can_num_drivers; i++) {
AP_DroneCAN *dronecan = AP_DroneCAN::get_dronecan(i);
if (dronecan == nullptr) {
continue;
}
if (dronecan->get_driver_index()+1 != _can_driver) {
continue;
}
// send messages
dronecan_send(dronecan);
}
#endif
}
// local payload space check which treats invalid channel as having space

1
libraries/AP_OpenDroneID/AP_OpenDroneID.h
View File

@ -185,6 +185,7 @@ private:
// mask of what UAVCAN drivers need to send each packet
const uint8_t dronecan_send_all = (1U<<HAL_MAX_CAN_PROTOCOL_DRIVERS)-1;
uint8_t driver_mask;
uint8_t need_send_location;
uint8_t need_send_basic_id;
uint8_t need_send_system;

10
libraries/AP_OpenDroneID/AP_OpenDroneID_DroneCAN.cpp
View File

@ -36,7 +36,7 @@ static void handle_arm_status(AP_DroneCAN* ap_dronecan, const CanardRxTransfer &
void AP_OpenDroneID::dronecan_init(AP_DroneCAN *uavcan)
{
const uint8_t driver_index = uavcan->get_driver_index();
const uint8_t driver_mask = 1U<<driver_index;
driver_mask = 1U<<driver_index;
if (dronecan_done_init & driver_mask) {
// already initialised
return;
@ -95,14 +95,6 @@ alloc_failed:
*/
void AP_OpenDroneID::dronecan_send(AP_DroneCAN *uavcan)
{
const uint8_t driver_index = uavcan->get_driver_index();
const uint8_t driver_mask = 1U<<driver_index;
if (driver_index+1 != _can_driver) {
// not enabled for this CAN driver
return;
}
dronecan_init(uavcan);
if (dronecan_init_failed & driver_mask) {