CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp
Views: 1799
1
/*

2
  simple DroneCAN network sniffer as an ArduPilot firmware

3
 */

4
#include <AP_Common/AP_Common.h>

5
#include <AP_HAL/AP_HAL.h>

6


7
#if HAL_ENABLE_DRONECAN_DRIVERS

8


9
#include <AP_HAL/Semaphores.h>

10


11
#include <AP_DroneCAN/AP_DroneCAN.h>

12


13
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX

14
#include <AP_HAL_Linux/CANSocketIface.h>

15
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL

16
#include <AP_HAL_SITL/CANSocketIface.h>

17
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS

18
#include <hal.h>

19
#include <AP_HAL_ChibiOS/CANIface.h>

20
#endif

21


22
void setup();

23
void loop();

24


25
const AP_HAL::HAL& hal = AP_HAL::get_HAL();

26


27
#define DRONECAN_NODE_POOL_SIZE 8192

28


29
static uint8_t node_memory_pool[DRONECAN_NODE_POOL_SIZE];

30


31
#define debug_dronecan(fmt, args...) do { hal.console->printf(fmt, ##args); } while (0)

32


33
class DroneCAN_sniffer {

34
public:

35
    DroneCAN_sniffer();

36
    ~DroneCAN_sniffer();

37


38
    void init(void);

39
    void loop(void);

40
    void print_stats(void);

41


42
private:

43
    uint8_t driver_index = 0;

44


45
    AP_CANManager can_mgr;

46
};

47


48
static struct {

49
    const char *msg_name;

50
    uint32_t count;

51
    uint64_t last_time_us;

52
    uint32_t avg_period_us;

53
    uint32_t max_period_us;

54
    uint32_t min_period_us;

55
} counters[100];

56


57
static void count_msg(const char *name)

58
{

59
    for (uint16_t i=0; i<ARRAY_SIZE(counters); i++) {

60
        if (counters[i].msg_name == name) {

61
            counters[i].count++;

62
            uint64_t now = AP_HAL::micros64();

63
            uint32_t period_us = now - counters[i].last_time_us;

64
            counters[i].last_time_us = now;

65
            if (counters[i].avg_period_us == 0) {

66
                counters[i].avg_period_us = period_us;

67
            } else {

68
                counters[i].avg_period_us = (counters[i].avg_period_us * (counters[i].count - 1) + period_us) / counters[i].count;

69
            }

70
            if (counters[i].max_period_us < period_us) {

71
                counters[i].max_period_us = period_us;

72
            }

73
            if (counters[i].min_period_us == 0 || counters[i].min_period_us > period_us) {

74
                counters[i].min_period_us = period_us;

75
            }

76
            break;

77
        }

78
        if (counters[i].msg_name == nullptr) {

79
            counters[i].msg_name = name;

80
            counters[i].count++;

81
            counters[i].last_time_us = AP_HAL::micros64();

82
            break;

83
        }

84
    }

85
}

86


87
#define MSG_CB(mtype, cbname)                                                  \

88
    static void cb_ ## cbname(const CanardRxTransfer &transfer, const mtype& msg) { count_msg(#mtype); }

89


90
MSG_CB(uavcan_protocol_NodeStatus, NodeStatus)

91
MSG_CB(uavcan_equipment_gnss_Fix2, Fix2)

92
MSG_CB(uavcan_equipment_gnss_Auxiliary, Auxiliary)

93
MSG_CB(uavcan_equipment_ahrs_MagneticFieldStrength, MagneticFieldStrength)

94
MSG_CB(uavcan_equipment_ahrs_MagneticFieldStrength2, MagneticFieldStrength2);

95
MSG_CB(uavcan_equipment_air_data_StaticPressure, StaticPressure)

96
MSG_CB(uavcan_equipment_air_data_StaticTemperature, StaticTemperature)

97
MSG_CB(uavcan_equipment_power_BatteryInfo, BatteryInfo);

98
MSG_CB(uavcan_equipment_actuator_ArrayCommand, ArrayCommand)

99
MSG_CB(uavcan_equipment_esc_RawCommand, RawCommand)

100
MSG_CB(uavcan_equipment_indication_LightsCommand, LightsCommand);

101
MSG_CB(com_hex_equipment_flow_Measurement, Measurement);

102


103


104
uavcan_protocol_NodeStatus node_status;

105
uavcan_protocol_GetNodeInfoResponse node_info;

106
CanardInterface *_uavcan_iface_mgr;

107
Canard::Publisher<uavcan_protocol_NodeStatus> *node_status_pub;

108
Canard::Server<uavcan_protocol_GetNodeInfoRequest> *node_info_srv;

109


110
static void cb_GetNodeInfoRequest(const CanardRxTransfer &transfer, const uavcan_protocol_GetNodeInfoRequest& msg)

111
{

112
    if (node_info_srv == nullptr) {

113
        return;

114
    }

115
    node_info_srv->respond(transfer, node_info);

116
}

117


118
void DroneCAN_sniffer::init(void)

119
{

120
    // we need to mutate the HAL to install new CAN interfaces

121
    AP_HAL::HAL& hal_mutable = AP_HAL::get_HAL_mutable();

122


123
    hal_mutable.can[driver_index] = NEW_NOTHROW HAL_CANIface(driver_index);

124
    

125
    if (hal_mutable.can[driver_index] == nullptr) {

126
        AP_HAL::panic("Couldn't allocate CANManager, something is very wrong");

127
    }

128


129
    hal_mutable.can[driver_index]->init(1000000, AP_HAL::CANIface::NormalMode);

130


131
    if (!hal_mutable.can[driver_index]->is_initialized()) {

132
        debug_dronecan("Can not initialised\n");

133
        return;

134
    }

135
    _uavcan_iface_mgr = NEW_NOTHROW CanardInterface{driver_index};

136


137
    if (_uavcan_iface_mgr == nullptr) {

138
        return;

139
    }

140


141
    if (!_uavcan_iface_mgr->add_interface(hal_mutable.can[driver_index])) {

142
        debug_dronecan("Failed to add iface");

143
        return;

144
    }

145


146
    _uavcan_iface_mgr->init(node_memory_pool, sizeof(node_memory_pool), 9);

147


148
    node_status_pub = NEW_NOTHROW Canard::Publisher<uavcan_protocol_NodeStatus>{*_uavcan_iface_mgr};

149
    if (node_status_pub == nullptr) {

150
        return;

151
    }

152


153
    node_info_srv = NEW_NOTHROW Canard::Server<uavcan_protocol_GetNodeInfoRequest>{*_uavcan_iface_mgr, *Canard::allocate_static_callback(cb_GetNodeInfoRequest)};

154
    if (node_info_srv == nullptr) {

155
        return;

156
    }

157


158
    node_info.name.len = snprintf((char*)node_info.name.data, sizeof(node_info.name.data), "org.ardupilot:%u", driver_index);

159


160
    node_info.software_version.major = AP_DRONECAN_SW_VERS_MAJOR;

161
    node_info.software_version.minor = AP_DRONECAN_SW_VERS_MINOR;

162


163
    node_info.hardware_version.major = AP_DRONECAN_HW_VERS_MAJOR;

164
    node_info.hardware_version.minor = AP_DRONECAN_HW_VERS_MINOR;

165


166
#define START_CB(mtype, cbname) Canard::allocate_sub_static_callback(cb_ ## cbname,driver_index)

167


168
    START_CB(uavcan_protocol_NodeStatus, NodeStatus);

169
    START_CB(uavcan_equipment_gnss_Fix2, Fix2);

170
    START_CB(uavcan_equipment_gnss_Auxiliary, Auxiliary);

171
    START_CB(uavcan_equipment_ahrs_MagneticFieldStrength, MagneticFieldStrength);

172
    START_CB(uavcan_equipment_ahrs_MagneticFieldStrength2, MagneticFieldStrength2);

173
    START_CB(uavcan_equipment_air_data_StaticPressure, StaticPressure);

174
    START_CB(uavcan_equipment_air_data_StaticTemperature, StaticTemperature);

175
    START_CB(uavcan_equipment_power_BatteryInfo, BatteryInfo);

176
    START_CB(uavcan_equipment_actuator_ArrayCommand, ArrayCommand);

177
    START_CB(uavcan_equipment_esc_RawCommand, RawCommand);

178
    START_CB(uavcan_equipment_indication_LightsCommand, LightsCommand);

179
    START_CB(com_hex_equipment_flow_Measurement, Measurement);

180


181
    debug_dronecan("DroneCAN: init done\n\r");

182
}

183


184
static void send_node_status()

185
{

186
    uavcan_protocol_NodeStatus msg;

187
    msg.uptime_sec = AP_HAL::millis() / 1000;

188
    msg.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;

189
    msg.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;

190
    msg.sub_mode = 0;

191
    msg.vendor_specific_status_code = 0;

192
    node_status_pub->broadcast(msg);

193
}

194


195
uint32_t last_status_send = 0;

196
void DroneCAN_sniffer::loop(void)

197
{

198
    if (AP_HAL::millis() - last_status_send > 1000) {

199
        last_status_send = AP_HAL::millis();

200
        send_node_status();

201
    }

202
    _uavcan_iface_mgr->process(1);

203
}

204


205
void DroneCAN_sniffer::print_stats(void)

206
{

207
    hal.console->printf("%lu\n", (unsigned long)AP_HAL::micros());

208
    for (uint16_t i=0;i<100;i++) {

209
        if (counters[i].msg_name == nullptr) {

210
            break;

211
        }

212
        hal.console->printf("%s: %lu AVG_US: %lu MAX_US: %lu MIN_US: %lu\n", counters[i].msg_name,

213
                                                                            (long unsigned)counters[i].count,

214
                                                                            (long unsigned)counters[i].avg_period_us,

215
                                                                            (long unsigned)counters[i].max_period_us,

216
                                                                            (long unsigned)counters[i].min_period_us);

217
        counters[i].count = 0;

218
        counters[i].avg_period_us = 0;

219
        counters[i].max_period_us = 0;

220
        counters[i].min_period_us = 0;

221
    }

222
    hal.console->printf("\n");

223
}

224


225
static DroneCAN_sniffer sniffer;

226


227
DroneCAN_sniffer::DroneCAN_sniffer()

228
{}

229


230
DroneCAN_sniffer::~DroneCAN_sniffer()

231
{

232
}

233


234
void setup(void)

235
{

236
    hal.scheduler->delay(2000);

237
    hal.console->printf("Starting DroneCAN sniffer\n");

238
    sniffer.init();

239
}

240


241
void loop(void)

242
{

243
    sniffer.loop();

244
    static uint32_t last_print_ms;

245
    uint32_t now = AP_HAL::millis();

246
    if (now - last_print_ms >= 1000) {

247
        last_print_ms = now;

248
        sniffer.print_stats();

249
    }

250


251
    // auto-reboot for --upload

252
    if (hal.console->available() > 50) {

253
        hal.console->printf("rebooting\n");

254
        hal.console->discard_input();

255
        hal.scheduler->reboot();

256
    }

257
    hal.console->discard_input();

258
}

259


260
AP_HAL_MAIN();

261


262
#else

263


264
#include <stdio.h>

265


266
const AP_HAL::HAL& hal = AP_HAL::get_HAL();

267


268
static void loop() { }

269
static void setup()

270
{

271
    printf("Board not currently supported\n");

272
}

273


274
AP_HAL_MAIN();

275
#endif

276


277