mirror of https://github.com/ArduPilot/ardupilot
AP_DroneCAN: rename UAVCAN drivers to DroneCAN
This commit is contained in:
Andrew Tridgell
parent
2e8ea6ac47
commit
9d37083ddf
|
|
@ -0,0 +1,27 @@
|
|||
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
|
||||
|
|
@ -0,0 +1,246 @@
|
|||
/*
|
||||
simple UAVCAN network sniffer as an ArduPilot firmware
|
||||
*/
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
|
||||
|
||||
#include <AP_HAL/Semaphores.h>
|
||||
|
||||
#include <AP_DroneCAN/AP_DroneCAN.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||
#include <AP_HAL_Linux/CANSocketIface.h>
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
#include <AP_HAL_SITL/CANSocketIface.h>
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
||||
#include <hal.h>
|
||||
#include <AP_HAL_ChibiOS/CANIface.h>
|
||||
#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<ARRAY_SIZE(counters); i++) {
|
||||
if (counters[i].msg_name == name) {
|
||||
counters[i].count++;
|
||||
break;
|
||||
}
|
||||
if (counters[i].msg_name == nullptr) {
|
||||
counters[i].msg_name = name;
|
||||
counters[i].count++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#define MSG_CB(mtype, cbname) \
|
||||
static void cb_ ## cbname(const CanardRxTransfer &transfer, const mtype& msg) { count_msg(#mtype); }
|
||||
|
||||
MSG_CB(uavcan_protocol_NodeStatus, NodeStatus)
|
||||
MSG_CB(uavcan_equipment_gnss_Fix2, Fix2)
|
||||
MSG_CB(uavcan_equipment_gnss_Auxiliary, Auxiliary)
|
||||
MSG_CB(uavcan_equipment_ahrs_MagneticFieldStrength, MagneticFieldStrength)
|
||||
MSG_CB(uavcan_equipment_ahrs_MagneticFieldStrength2, MagneticFieldStrength2);
|
||||
MSG_CB(uavcan_equipment_air_data_StaticPressure, StaticPressure)
|
||||
MSG_CB(uavcan_equipment_air_data_StaticTemperature, StaticTemperature)
|
||||
MSG_CB(uavcan_equipment_power_BatteryInfo, BatteryInfo);
|
||||
MSG_CB(uavcan_equipment_actuator_ArrayCommand, ArrayCommand)
|
||||
MSG_CB(uavcan_equipment_esc_RawCommand, RawCommand)
|
||||
MSG_CB(uavcan_equipment_indication_LightsCommand, LightsCommand);
|
||||
MSG_CB(com_hex_equipment_flow_Measurement, Measurement);
|
||||
|
||||
|
||||
uavcan_protocol_NodeStatus node_status;
|
||||
uavcan_protocol_GetNodeInfoResponse node_info;
|
||||
CanardInterface *_uavcan_iface_mgr;
|
||||
Canard::Publisher<uavcan_protocol_NodeStatus> *node_status_pub;
|
||||
Canard::Server<uavcan_protocol_GetNodeInfoRequest> *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 <AP_HAL::HAL&> (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_protocol_NodeStatus>{*_uavcan_iface_mgr};
|
||||
if (node_status_pub == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
node_info_srv = new Canard::Server<uavcan_protocol_GetNodeInfoRequest>{*_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 <stdio.h>
|
||||
|
||||
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
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
#!/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'],
|
||||
)
|
||||
Loading…
Reference in New Issue