From 60beb288eeae02e016e505012a91ea521b739a4b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 8 Apr 2023 13:55:40 +1000 Subject: [PATCH] AP_DroneCAN: update header references --- libraries/AP_DroneCAN/AP_DroneCAN.cpp | 20 +- .../examples/UAVCAN_sniffer/README.md | 27 -- .../UAVCAN_sniffer/UAVCAN_sniffer.cpp | 246 ------------------ .../examples/UAVCAN_sniffer/nobuild.txt | 0 .../examples/UAVCAN_sniffer/wscript | 21 -- 5 files changed, 10 insertions(+), 304 deletions(-) delete mode 100644 libraries/AP_DroneCAN/examples/UAVCAN_sniffer/README.md delete mode 100644 libraries/AP_DroneCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp delete mode 100644 libraries/AP_DroneCAN/examples/UAVCAN_sniffer/nobuild.txt delete mode 100644 libraries/AP_DroneCAN/examples/UAVCAN_sniffer/wscript diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 7a3651ce14..52782823b9 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -26,19 +26,19 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include -#include +#include #include -#include +#include #include -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/libraries/AP_DroneCAN/examples/UAVCAN_sniffer/README.md b/libraries/AP_DroneCAN/examples/UAVCAN_sniffer/README.md deleted file mode 100644 index 44f3e1c5b4..0000000000 --- a/libraries/AP_DroneCAN/examples/UAVCAN_sniffer/README.md +++ /dev/null @@ -1,27 +0,0 @@ -This is a UAVCAN sniffer designed to run on an ArduPilot board. It can -be used to watch traffic on a UAVCAN bus, showing exactly what would -be received by another node. - -To build and upload for a Pixhawk style board run this: - -``` - ./waf configure --board fmuv3 - ./waf --target examples/UAVCAN_sniffer --upload -``` - -then connect on the USB console. You will see 1Hz packet stats like -this: - -``` -uavcan.equipment.air_data.StaticPressure: 29 -uavcan.equipment.air_data.StaticTemperature: 29 -uavcan.equipment.ahrs.MagneticFieldStrength: 20 -uavcan.protocol.NodeStatus: 6 -uavcan.equipment.gnss.Fix: 10 -uavcan.equipment.gnss.Auxiliary: 1 -uavcan.equipment.actuator.ArrayCommand: 45 -uavcan.equipment.esc.RawCommand: 368 -``` - -note that the code requires you to add new msg types you want to -see. Look for the MSG_CB() and START_CB() macros in the code diff --git a/libraries/AP_DroneCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp b/libraries/AP_DroneCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp deleted file mode 100644 index 942404349b..0000000000 --- a/libraries/AP_DroneCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp +++ /dev/null @@ -1,246 +0,0 @@ -/* - simple UAVCAN network sniffer as an ArduPilot firmware - */ -#include -#include - -#if HAL_ENABLE_LIBUAVCAN_DRIVERS - -#include - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX -#include -#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL -#include -#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS -#include -#include -#endif - -void setup(); -void loop(); - -const AP_HAL::HAL& hal = AP_HAL::get_HAL(); - -#define UAVCAN_NODE_POOL_SIZE 8192 - -static uint8_t node_memory_pool[UAVCAN_NODE_POOL_SIZE]; - -#define debug_uavcan(fmt, args...) do { hal.console->printf(fmt, ##args); } while (0) - -class UAVCAN_sniffer { -public: - UAVCAN_sniffer(); - ~UAVCAN_sniffer(); - - void init(void); - void loop(void); - void print_stats(void); - -private: - uint8_t driver_index = 0; - - AP_CANManager can_mgr; -}; - -static struct { - const char *msg_name; - uint32_t count; -} counters[100]; - -static void count_msg(const char *name) -{ - for (uint16_t i=0; i *node_status_pub; -Canard::Server *node_info_srv; - -static void cb_GetNodeInfoRequest(const CanardRxTransfer &transfer, const uavcan_protocol_GetNodeInfoRequest& msg) -{ - if (node_info_srv == nullptr) { - return; - } - node_info_srv->respond(transfer, node_info); -} - -void UAVCAN_sniffer::init(void) -{ - const_cast (hal).can[driver_index] = new HAL_CANIface(driver_index); - - if (hal.can[driver_index] == nullptr) { - AP_HAL::panic("Couldn't allocate CANManager, something is very wrong"); - } - - hal.can[driver_index]->init(1000000, AP_HAL::CANIface::NormalMode); - - if (!hal.can[driver_index]->is_initialized()) { - debug_uavcan("Can not initialised\n"); - return; - } - _uavcan_iface_mgr = new CanardInterface{driver_index}; - - if (_uavcan_iface_mgr == nullptr) { - return; - } - - if (!_uavcan_iface_mgr->add_interface(hal.can[driver_index])) { - debug_uavcan("Failed to add iface"); - return; - } - - _uavcan_iface_mgr->init(node_memory_pool, sizeof(node_memory_pool), 9); - - node_status_pub = new Canard::Publisher{*_uavcan_iface_mgr}; - if (node_status_pub == nullptr) { - return; - } - - node_info_srv = new Canard::Server{*_uavcan_iface_mgr, *Canard::allocate_static_callback(cb_GetNodeInfoRequest)}; - if (node_info_srv == nullptr) { - return; - } - - node_info.name.len = snprintf((char*)node_info.name.data, sizeof(node_info.name.data), "org.ardupilot:%u", driver_index); - - node_info.software_version.major = AP_DRONECAN_SW_VERS_MAJOR; - node_info.software_version.minor = AP_DRONECAN_SW_VERS_MINOR; - - node_info.hardware_version.major = AP_DRONECAN_HW_VERS_MAJOR; - node_info.hardware_version.minor = AP_DRONECAN_HW_VERS_MINOR; - -#define START_CB(mtype, cbname) Canard::allocate_sub_static_callback(cb_ ## cbname,driver_index) - - START_CB(uavcan_protocol_NodeStatus, NodeStatus); - START_CB(uavcan_equipment_gnss_Fix2, Fix2); - START_CB(uavcan_equipment_gnss_Auxiliary, Auxiliary); - START_CB(uavcan_equipment_ahrs_MagneticFieldStrength, MagneticFieldStrength); - START_CB(uavcan_equipment_ahrs_MagneticFieldStrength2, MagneticFieldStrength2); - START_CB(uavcan_equipment_air_data_StaticPressure, StaticPressure); - START_CB(uavcan_equipment_air_data_StaticTemperature, StaticTemperature); - START_CB(uavcan_equipment_power_BatteryInfo, BatteryInfo); - START_CB(uavcan_equipment_actuator_ArrayCommand, ArrayCommand); - START_CB(uavcan_equipment_esc_RawCommand, RawCommand); - START_CB(uavcan_equipment_indication_LightsCommand, LightsCommand); - START_CB(com_hex_equipment_flow_Measurement, Measurement); - - debug_uavcan("UAVCAN: init done\n\r"); -} - -static void send_node_status() -{ - uavcan_protocol_NodeStatus msg; - msg.uptime_sec = AP_HAL::millis() / 1000; - msg.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; - msg.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; - msg.sub_mode = 0; - msg.vendor_specific_status_code = 0; - node_status_pub->broadcast(msg); -} - -uint32_t last_status_send = 0; -void UAVCAN_sniffer::loop(void) -{ - if (AP_HAL::millis() - last_status_send > 1000) { - last_status_send = AP_HAL::millis(); - send_node_status(); - } - _uavcan_iface_mgr->process(1); -} - -void UAVCAN_sniffer::print_stats(void) -{ - hal.console->printf("%lu\n", (unsigned long)AP_HAL::micros()); - for (uint16_t i=0;i<100;i++) { - if (counters[i].msg_name == nullptr) { - break; - } - hal.console->printf("%s: %u\n", counters[i].msg_name, unsigned(counters[i].count)); - counters[i].count = 0; - } - hal.console->printf("\n"); -} - -static UAVCAN_sniffer sniffer; - -UAVCAN_sniffer::UAVCAN_sniffer() -{} - -UAVCAN_sniffer::~UAVCAN_sniffer() -{ -} - -void setup(void) -{ - hal.scheduler->delay(2000); - hal.console->printf("Starting UAVCAN sniffer\n"); - sniffer.init(); -} - -void loop(void) -{ - sniffer.loop(); - static uint32_t last_print_ms; - uint32_t now = AP_HAL::millis(); - if (now - last_print_ms >= 1000) { - last_print_ms = now; - sniffer.print_stats(); - } - - // auto-reboot for --upload - if (hal.console->available() > 50) { - hal.console->printf("rebooting\n"); - hal.console->discard_input(); - hal.scheduler->reboot(false); - } - hal.console->discard_input(); -} - -AP_HAL_MAIN(); - -#else - -#include - -const AP_HAL::HAL& hal = AP_HAL::get_HAL(); - -static void loop() { } -static void setup() -{ - printf("Board not currently supported\n"); -} - -AP_HAL_MAIN(); -#endif diff --git a/libraries/AP_DroneCAN/examples/UAVCAN_sniffer/nobuild.txt b/libraries/AP_DroneCAN/examples/UAVCAN_sniffer/nobuild.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/libraries/AP_DroneCAN/examples/UAVCAN_sniffer/wscript b/libraries/AP_DroneCAN/examples/UAVCAN_sniffer/wscript deleted file mode 100644 index 3e81beeee1..0000000000 --- a/libraries/AP_DroneCAN/examples/UAVCAN_sniffer/wscript +++ /dev/null @@ -1,21 +0,0 @@ -#!/usr/bin/env python -# encoding: utf-8 -from waflib.TaskGen import after_method, before_method, feature - -def build(bld): - vehicle = bld.path.name - - bld.ap_stlib( - name=vehicle + '_libs', - ap_vehicle='UNKNOWN', - dynamic_source='modules/DroneCAN/libcanard/dsdlc_generated/src/**.c', - ap_libraries=bld.ap_common_vehicle_libraries() + [ - 'AP_OSD', - 'AP_RCMapper', - 'AP_Arming' - ], - ) - bld.ap_program( - program_groups=['tool'], - use=[vehicle + '_libs'], - )