From 33d3d34f3bcf0a4694951afced116d82888b65d2 Mon Sep 17 00:00:00 2001 From: Aashay Shah Date: Wed, 30 Oct 2024 22:27:03 -0700 Subject: [PATCH] Add --enable-overrid option - separated out the mac_eq function into a _utils file - We can add more defines using --enable-overrid. The current one is AP_ODIDSCANNER_ENABLED --- Tools/ardupilotwaf/boards.py | 11 +++++++- libraries/AC_Avoidance/AP_OABendyRuler.cpp | 6 ++++- libraries/AP_Avoidance/AP_Avoidance.cpp | 5 ++++ libraries/AP_ODIDScanner/AP_ODIDScanner.cpp | 26 ++++++++----------- libraries/AP_ODIDScanner/AP_ODIDScanner.h | 18 ++++++++----- .../AP_ODIDScanner/AP_ODIDScanner_config.h | 8 ++++++ .../AP_ODIDScanner/AP_ODIDScanner_utils.cpp | 10 +++++++ .../AP_ODIDScanner/AP_ODIDScanner_utils.h | 4 +++ libraries/AP_Vehicle/AP_Vehicle.cpp | 6 ++++- libraries/AP_Vehicle/AP_Vehicle.h | 2 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 4 +-- wscript | 5 +++- 12 files changed, 78 insertions(+), 27 deletions(-) create mode 100644 libraries/AP_ODIDScanner/AP_ODIDScanner_config.h create mode 100644 libraries/AP_ODIDScanner/AP_ODIDScanner_utils.cpp create mode 100644 libraries/AP_ODIDScanner/AP_ODIDScanner_utils.h diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 426f01baae5e6..aaedcbc96021a 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -104,6 +104,15 @@ def srcpath(path): else: cfg.msg("Enabled OpenDroneID", 'no', color='YELLOW') + if cfg.options.enable_overrid: + env.ENABLE_OVERRID = True + env.DEFINES.update( + AP_ODIDSCANNER_ENABLED=1, + ) + cfg.msg("Enabled uci-overRID", 'yes') + else: + cfg.msg("Enabled uci-overRID", 'no', color='YELLOW') + # allow enable of firmware ID checking for any board if cfg.options.enable_check_firmware: env.CHECK_FIRMWARE_ENABLED = True @@ -1400,4 +1409,4 @@ class SITL_x86_64_linux_gnu(SITL_static): toolchain = 'x86_64-linux-gnu' class SITL_arm_linux_gnueabihf(SITL_static): - toolchain = 'arm-linux-gnueabihf' + toolchain = 'arm-linux-gnueabihf' \ No newline at end of file diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.cpp b/libraries/AC_Avoidance/AP_OABendyRuler.cpp index 33b6229f4e9b0..cf5c1032f3647 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.cpp +++ b/libraries/AC_Avoidance/AP_OABendyRuler.cpp @@ -451,9 +451,11 @@ float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Locati margin_min = MIN(margin_min, latest_margin); } +#if AP_ODIDSCANNER_ENABLED if (calc_margin_from_open_drone_id(start, end, latest_margin)) { margin_min = MIN(margin_min, latest_margin); } +#endif // return smallest margin from any obstacle return margin_min; @@ -715,6 +717,7 @@ bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, co return false; } +#if AP_ODIDSCANNER_ENABLED bool AP_OABendyRuler::calc_margin_from_open_drone_id(const Location &start, const Location &end, float &margin) const { AP_ODIDScanner* odid = &AP::vehicle()->odidscanner; @@ -745,4 +748,5 @@ bool AP_OABendyRuler::calc_margin_from_open_drone_id(const Location &start, cons } return false; -} \ No newline at end of file +} +#endif diff --git a/libraries/AP_Avoidance/AP_Avoidance.cpp b/libraries/AP_Avoidance/AP_Avoidance.cpp index ce7e893d46827..0af8c27b00c95 100644 --- a/libraries/AP_Avoidance/AP_Avoidance.cpp +++ b/libraries/AP_Avoidance/AP_Avoidance.cpp @@ -10,6 +10,7 @@ extern const AP_HAL::HAL& hal; #include #include #include +#include #define AVOIDANCE_DEBUGGING 0 @@ -342,6 +343,7 @@ void AP_Avoidance::get_adsb_samples() void AP_Avoidance::get_odid_samples() { +#if AP_ODIDSCANNER_ENABLED //TODO: Process samples here // GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Avoidance: Get %d drones", AP::vehicle()->odidscanner.get_count()); for(int i = 0;iodidscanner.get_count();i++) { @@ -359,6 +361,7 @@ void AP_Avoidance::get_odid_samples() AP::vehicle()->odidscanner.get_vehicle_location(i), vel); } +#endif } float closest_approach_xy(const Location &my_loc, @@ -639,9 +642,11 @@ void AP_Avoidance::update() if (_adsb.enabled()) { get_adsb_samples(); } +#if AP_ODIDSCANNER_ENABLED if (AP::vehicle()->odidscanner.enabled()) { get_odid_samples(); } +#endif check_for_threats(); diff --git a/libraries/AP_ODIDScanner/AP_ODIDScanner.cpp b/libraries/AP_ODIDScanner/AP_ODIDScanner.cpp index 58f905ea85928..039ccc2383448 100644 --- a/libraries/AP_ODIDScanner/AP_ODIDScanner.cpp +++ b/libraries/AP_ODIDScanner/AP_ODIDScanner.cpp @@ -1,4 +1,7 @@ #include "AP_ODIDScanner.h" + +#if AP_ODIDSCANNER_ENABLED + #include "GCS_MAVLink/GCS_MAVLink.h" #include "GCS_MAVLink/GCS.h" #include @@ -18,14 +21,6 @@ #define VEHICLE_TIMEOUT_MS 30000 -bool mac_eq(uint8_t a[6], uint8_t b[6]) { - return a[0] == b[0] && - a[1] == b[1] && - a[2] == b[2] && - a[3] == b[3] && - a[4] == b[4] && - a[5] == b[5]; -} // TODO: Random default for mav_port needs fix AP_ODIDScanner::AP_ODIDScanner() : _mav_port(1){ @@ -37,11 +32,11 @@ bool AP_ODIDScanner::enabled() { void AP_ODIDScanner::init() { _chan = mavlink_channel_t(gcs().get_channel_from_port_number(_mav_port)); _initialised = true; - _port = AP::serialmanager().get_serial_by_id(_mav_port); - if (_port != nullptr) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Scanner: Found RID Device"); - _port->begin(57600, 512, 512); - } + /*_port = AP::serialmanager().get_serial_by_id(_mav_port);*/ + /*if (_port != nullptr) {*/ + /* GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Scanner: Found RID Device");*/ + /* _port->begin(57600, 512, 512);*/ + /*}*/ } @@ -92,7 +87,7 @@ void AP_ODIDScanner::update() { last_dev_hb_msg_ms = now_ms; // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Scanner: Device Not Found at %f",float(now_ms)); // this is a bug, the device is working when this is sent.... - _port->printf("Scanner: Where is this printing?"); + /*_port->printf("Scanner: Where is this printing?");*/ } if (now_ms - last_hb_send_ms > 1000) { // why this? @@ -251,4 +246,5 @@ rid_vehicle_t& AP_ODIDScanner::get_vehicle(int i) { Location AP_ODIDScanner::get_vehicle_location(int i) { auto v = this->get_vehicle(i); return Location(v.loc.latitude, v.loc.longitude, v.loc.altitude_geodetic*100, Location::AltFrame::ABSOLUTE); -} \ No newline at end of file +} +#endif // AP_ODIDSCANNER_ENABLED \ No newline at end of file diff --git a/libraries/AP_ODIDScanner/AP_ODIDScanner.h b/libraries/AP_ODIDScanner/AP_ODIDScanner.h index a412db87c899c..4c8ff4928274f 100644 --- a/libraries/AP_ODIDScanner/AP_ODIDScanner.h +++ b/libraries/AP_ODIDScanner/AP_ODIDScanner.h @@ -1,9 +1,15 @@ #pragma once -#include -#define ODID_SCANNER_ENABLED 1 -#ifdef ODID_SCANNER_ENABLED +#ifndef AP_ODIDSCANNER_ENABLED +// default to off. Enabled in hwdef.dat +#define AP_ODIDSCANNER_ENABLED 0 +#endif + +#if AP_ODIDSCANNER_ENABLED +/*#include */ +/*#define ODID_SCANNER_ENABLED 1*/ +/*#ifdef ODID_SCANNER_ENABLED*/ #include "AP_Common/AP_Common.h" #include @@ -11,8 +17,8 @@ #include #include #include +#include "AP_ODIDScanner_utils.h" -extern bool mac_eq(uint8_t a[6], uint8_t b[6]); struct Loc : Location { @@ -118,7 +124,7 @@ class AP_ODIDScanner mavlink_uav_found_t found_msg; mavlink_channel_t _chan; - AP_HAL::UARTDriver* _port; + /*AP_HAL::UARTDriver* _port;*/ bool _initialised; uint32_t last_send_ms; uint32_t last_dev_hb_ms; @@ -133,4 +139,4 @@ class AP_ODIDScanner }; -#endif +#endif // AP_ODIDSCANNER_ENABLED \ No newline at end of file diff --git a/libraries/AP_ODIDScanner/AP_ODIDScanner_config.h b/libraries/AP_ODIDScanner/AP_ODIDScanner_config.h new file mode 100644 index 0000000000000..555779f556dd4 --- /dev/null +++ b/libraries/AP_ODIDScanner/AP_ODIDScanner_config.h @@ -0,0 +1,8 @@ +#pragma once + +#include + +#ifndef AP_ODIDSCANNER_ENABLED +// default to off. Enabled in hwdef.dat +#define AP_ODIDSCANNER_ENABLED 1 +#endif \ No newline at end of file diff --git a/libraries/AP_ODIDScanner/AP_ODIDScanner_utils.cpp b/libraries/AP_ODIDScanner/AP_ODIDScanner_utils.cpp new file mode 100644 index 0000000000000..3b70a52bbd7bb --- /dev/null +++ b/libraries/AP_ODIDScanner/AP_ODIDScanner_utils.cpp @@ -0,0 +1,10 @@ +#include"AP_ODIDScanner_utils.h" + +bool mac_eq(uint8_t a[6], uint8_t b[6]) { + return a[0] == b[0] && + a[1] == b[1] && + a[2] == b[2] && + a[3] == b[3] && + a[4] == b[4] && + a[5] == b[5]; +} diff --git a/libraries/AP_ODIDScanner/AP_ODIDScanner_utils.h b/libraries/AP_ODIDScanner/AP_ODIDScanner_utils.h new file mode 100644 index 0000000000000..dc4621503a5ef --- /dev/null +++ b/libraries/AP_ODIDScanner/AP_ODIDScanner_utils.h @@ -0,0 +1,4 @@ +#pragma once +#include + +bool mac_eq(uint8_t a[6], uint8_t b[6]); \ No newline at end of file diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 4f36ecea844c5..3a641b490dcfc 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -266,7 +266,9 @@ void AP_Vehicle::setup() #if AP_OPENDRONEID_ENABLED opendroneid.init(); #endif +#if AP_ODIDSCANNER_ENABLED odidscanner.init(); +#endif // init EFI monitoring #if HAL_EFI_ENABLED @@ -398,7 +400,9 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_OpenDroneID, &vehicle.opendroneid, update, 10, 50, 236), #endif // test comment... +#if AP_ODIDSCANNER_ENABLED SCHED_TASK_CLASS(AP_ODIDScanner,&vehicle.odidscanner, update, 10, 50, 237), +#endif #if OSD_ENABLED SCHED_TASK(publish_osd_info, 1, 10, 240), #endif @@ -834,4 +838,4 @@ AP_Vehicle *vehicle() return AP_Vehicle::get_singleton(); } -}; +}; \ No newline at end of file diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index fc86f4d414473..7fdf6ede487a8 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -327,7 +327,9 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { AP_OpenDroneID opendroneid; #endif public: +#if AP_ODIDSCANNER_ENABLED AP_ODIDScanner odidscanner; +#endif #if HAL_MSP_ENABLED AP_MSP msp; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index fdcf49fbc333a..a516b6d8d539f 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4016,7 +4016,7 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) #endif break; -// #if AP_OPENDRONEID_ENABLED +#if AP_ODIDSCANNER_ENABLED case MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS: case MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID: case MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID: @@ -4032,10 +4032,10 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) AP::opendroneid().handle_msg(chan, msg); } break; -// #endif case MAVLINK_MSG_ID_UAV_FOUND: AP::vehicle()->odidscanner.handle_msg(msg); break; +#endif #if AP_SIGNED_FIRMWARE case MAVLINK_MSG_ID_SECURE_COMMAND: diff --git a/wscript b/wscript index f3b18ebb06121..8df6b3444f2f5 100644 --- a/wscript +++ b/wscript @@ -243,6 +243,9 @@ submodules at specific revisions. g.add_option('--enable-opendroneid', action='store_true', default=False, help="Enables OpenDroneID") + g.add_option('--enable-overrid', action='store_true', + default=False, + help="Enables OpenDroneID") g.add_option('--enable-check-firmware', action='store_true', default=False, @@ -895,4 +898,4 @@ class RsyncContext(LocalInstallContext): if 'RSYNC_DEST' not in tg.env: self.fatal('Destination for rsync not defined. Either pass --rsync-dest here or during configuration.') - tg.post() + tg.post() \ No newline at end of file