CoCalc -- AP_DroneCAN.cpp

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/AP_DroneCAN.cpp
Views: ¹⁷⁹⁸
1
/*
2
 * This file is free software: you can redistribute it and/or modify it
3
 * under the terms of the GNU General Public License as published by the
4
 * Free Software Foundation, either version 3 of the License, or
5
 * (at your option) any later version.
6
 *
7
 * This file is distributed in the hope that it will be useful, but
8
 * WITHOUT ANY WARRANTY; without even the implied warranty of
9
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
10
 * See the GNU General Public License for more details.
11
 *
12
 * You should have received a copy of the GNU General Public License along
13
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
14
 *
15
 * Author: Eugene Shamaev, Siddharth Bharat Purohit
16
 */
17

18
#include <AP_Common/AP_Common.h>
19
#include <AP_HAL/AP_HAL.h>
20

21
#if HAL_ENABLE_DRONECAN_DRIVERS
22
#include "AP_DroneCAN.h"
23
#include <GCS_MAVLink/GCS.h>
24

25
#include <AP_BoardConfig/AP_BoardConfig.h>
26
#include <AP_CANManager/AP_CANManager.h>
27

28
#include <AP_Arming/AP_Arming.h>
29
#include <AP_GPS/AP_GPS_DroneCAN.h>
30
#include <AP_Compass/AP_Compass_DroneCAN.h>
31
#include <AP_Baro/AP_Baro_DroneCAN.h>
32
#include <AP_Vehicle/AP_Vehicle.h>
33
#include <AP_BattMonitor/AP_BattMonitor_DroneCAN.h>
34
#include <AP_Airspeed/AP_Airspeed_DroneCAN.h>
35
#include <AP_OpticalFlow/AP_OpticalFlow_HereFlow.h>
36
#include <AP_RangeFinder/AP_RangeFinder_DroneCAN.h>
37
#include <AP_RCProtocol/AP_RCProtocol_DroneCAN.h>
38
#include <AP_EFI/AP_EFI_DroneCAN.h>
39
#include <AP_GPS/AP_GPS_DroneCAN.h>
40
#include <AP_GPS/AP_GPS.h>
41
#include <AP_BattMonitor/AP_BattMonitor_DroneCAN.h>
42
#include <AP_Compass/AP_Compass_DroneCAN.h>
43
#include <AP_Airspeed/AP_Airspeed_DroneCAN.h>
44
#include <AP_Proximity/AP_Proximity_DroneCAN.h>
45
#include <SRV_Channel/SRV_Channel.h>
46
#include <AP_ADSB/AP_ADSB.h>
47
#include "AP_DroneCAN_DNA_Server.h"
48
#include <AP_Logger/AP_Logger.h>
49
#include <AP_Notify/AP_Notify.h>
50
#include <AP_OpenDroneID/AP_OpenDroneID.h>
51
#include <AP_Mount/AP_Mount_Xacti.h>
52
#include <string.h>
53
#include <AP_Servo_Telem/AP_Servo_Telem.h>
54

55
#if AP_DRONECAN_SERIAL_ENABLED
56
#include "AP_DroneCAN_serial.h"
57
#endif
58

59
#if AP_RELAY_DRONECAN_ENABLED
60
#include <AP_Relay/AP_Relay.h>
61
#endif
62

63
#include <AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.h>
64

65
#include <AP_RPM/RPM_DroneCAN.h>
66

67
extern const AP_HAL::HAL& hal;
68

69
// setup default pool size
70
#ifndef DRONECAN_NODE_POOL_SIZE
71
#if HAL_CANFD_SUPPORTED
72
#define DRONECAN_NODE_POOL_SIZE 16384
73
#else
74
#define DRONECAN_NODE_POOL_SIZE 8192
75
#endif
76
#endif
77

78
#if HAL_CANFD_SUPPORTED
79
#define DRONECAN_STACK_SIZE     8192
80
#else
81
#define DRONECAN_STACK_SIZE     4096
82
#endif
83

84
#ifndef AP_DRONECAN_DEFAULT_NODE
85
#define AP_DRONECAN_DEFAULT_NODE 10
86
#endif
87

88
#define AP_DRONECAN_GETSET_TIMEOUT_MS 100       // timeout waiting for response from node after 0.1 sec
89

90
#define debug_dronecan(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "DroneCAN", fmt, ##args); } while (0)
91

92
// Translation of all messages from DroneCAN structures into AP structures is done
93
// in AP_DroneCAN and not in corresponding drivers.
94
// The overhead of including definitions of DSDL is very high and it is best to
95
// concentrate in one place.
96

97
// table of user settable CAN bus parameters
98
const AP_Param::GroupInfo AP_DroneCAN::var_info[] = {
99
    // @Param: NODE
100
    // @DisplayName: Own node ID
101
    // @Description: DroneCAN node ID used by the driver itself on this network
102
    // @Range: 1 125
103
    // @User: Advanced
104
    AP_GROUPINFO("NODE", 1, AP_DroneCAN, _dronecan_node, AP_DRONECAN_DEFAULT_NODE),
105

106
    // @Param: SRV_BM
107
    // @DisplayName: Output channels to be transmitted as servo over DroneCAN
108
    // @Description: Bitmask with one set for channel to be transmitted as a servo command over DroneCAN
109
    // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15, 15: Servo 16, 16: Servo 17, 17: Servo 18, 18: Servo 19, 19: Servo 20, 20: Servo 21, 21: Servo 22, 22: Servo 23, 23: Servo 24, 24: Servo 25, 25: Servo 26, 26: Servo 27, 27: Servo 28, 28: Servo 29, 29: Servo 30, 30: Servo 31, 31: Servo 32
110

111
    // @User: Advanced
112
    AP_GROUPINFO("SRV_BM", 2, AP_DroneCAN, _servo_bm, 0),
113

114
    // @Param: ESC_BM
115
    // @DisplayName: Output channels to be transmitted as ESC over DroneCAN
116
    // @Description: Bitmask with one set for channel to be transmitted as a ESC command over DroneCAN
117
    // @Bitmask: 0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16, 16: ESC 17, 17: ESC 18, 18: ESC 19, 19: ESC 20, 20: ESC 21, 21: ESC 22, 22: ESC 23, 23: ESC 24, 24: ESC 25, 25: ESC 26, 26: ESC 27, 27: ESC 28, 28: ESC 29, 29: ESC 30, 30: ESC 31, 31: ESC 32
118
    // @User: Advanced
119
    AP_GROUPINFO("ESC_BM", 3, AP_DroneCAN, _esc_bm, 0),
120

121
    // @Param: SRV_RT
122
    // @DisplayName: Servo output rate
123
    // @Description: Maximum transmit rate for servo outputs
124
    // @Range: 1 200
125
    // @Units: Hz
126
    // @User: Advanced
127
    AP_GROUPINFO("SRV_RT", 4, AP_DroneCAN, _servo_rate_hz, 50),
128

129
    // @Param: OPTION
130
    // @DisplayName: DroneCAN options
131
    // @Description: Option flags
132
    // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS,6:UseHimarkServo,7:HobbyWingESC,8:EnableStats,9:EnableFlexDebug
133
    // @User: Advanced
134
    AP_GROUPINFO("OPTION", 5, AP_DroneCAN, _options, 0),
135
    
136
    // @Param: NTF_RT
137
    // @DisplayName: Notify State rate
138
    // @Description: Maximum transmit rate for Notify State Message
139
    // @Range: 1 200
140
    // @Units: Hz
141
    // @User: Advanced
142
    AP_GROUPINFO("NTF_RT", 6, AP_DroneCAN, _notify_state_hz, 20),
143

144
    // @Param: ESC_OF
145
    // @DisplayName: ESC Output channels offset
146
    // @Description: Offset for ESC numbering in DroneCAN ESC RawCommand messages. This allows for more efficient packing of ESC command messages. If your ESCs are on servo functions 5 to 8 and you set this parameter to 4 then the ESC RawCommand will be sent with the first 4 slots filled. This can be used for more efficient usage of CAN bandwidth
147
    // @Range: 0 18
148
    // @User: Advanced
149
    AP_GROUPINFO("ESC_OF", 7, AP_DroneCAN, _esc_offset, 0),
150

151
    // @Param: POOL
152
    // @DisplayName: CAN pool size
153
    // @Description: Amount of memory in bytes to allocate for the DroneCAN memory pool. More memory is needed for higher CAN bus loads
154
    // @Range: 1024 16384
155
    // @User: Advanced
156
    AP_GROUPINFO("POOL", 8, AP_DroneCAN, _pool_size, DRONECAN_NODE_POOL_SIZE),
157

158
    // @Param: ESC_RV
159
    // @DisplayName: Bitmask for output channels for reversible ESCs over DroneCAN.
160
    // @Description: Bitmask with one set for each output channel that uses a reversible ESC over DroneCAN. Reversible ESCs use both positive and negative values in RawCommands, with positive commanding the forward direction and negative commanding the reverse direction.
161
    // @Bitmask: 0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16, 16: ESC 17, 17: ESC 18, 18: ESC 19, 19: ESC 20, 20: ESC 21, 21: ESC 22, 22: ESC 23, 23: ESC 24, 24: ESC 25, 25: ESC 26, 26: ESC 27, 27: ESC 28, 28: ESC 29, 29: ESC 30, 30: ESC 31, 31: ESC 32
162
    // @User: Advanced
163
    AP_GROUPINFO("ESC_RV", 9, AP_DroneCAN, _esc_rv, 0),
164

165
#if AP_RELAY_DRONECAN_ENABLED
166
    // @Param: RLY_RT
167
    // @DisplayName: DroneCAN relay output rate
168
    // @Description: Maximum transmit rate for relay outputs, note that this rate is per message each message does 1 relay, so if with more relays will take longer to update at the same rate, a extra message will be sent when a relay changes state
169
    // @Range: 0 200
170
    // @Units: Hz
171
    // @User: Advanced
172
    AP_GROUPINFO("RLY_RT", 23, AP_DroneCAN, _relay.rate_hz, 0),
173
#endif
174

175
#if AP_DRONECAN_SERIAL_ENABLED
176
    /*
177
      due to the parameter tree depth limitation we can't use a sub-table for the serial parameters
178
     */
179

180
    // @Param: SER_EN
181
    // @DisplayName: DroneCAN Serial enable
182
    // @Description: Enable DroneCAN virtual serial ports
183
    // @Values: 0:Disabled, 1:Enabled
184
    // @RebootRequired: True
185
    // @User: Advanced
186
    AP_GROUPINFO_FLAGS("SER_EN", 10,  AP_DroneCAN, serial.enable, 0, AP_PARAM_FLAG_ENABLE),
187

188
    // @Param: S1_NOD
189
    // @DisplayName: Serial CAN remote node number
190
    // @Description: CAN remote node number for serial port
191
    // @Range: 0 127
192
    // @RebootRequired: True
193
    // @User: Advanced
194
    AP_GROUPINFO("S1_NOD", 11,  AP_DroneCAN, serial.ports[0].node, 0),
195

196
    // @Param: S1_IDX
197
    // @DisplayName: DroneCAN Serial1 index
198
    // @Description: Serial port number on remote CAN node
199
    // @Range: 0 100
200
    // @Values: -1:Disabled,0:Serial0,1:Serial1,2:Serial2,3:Serial3,4:Serial4,5:Serial5,6:Serial6
201
    // @RebootRequired: True
202
    // @User: Advanced
203
    AP_GROUPINFO("S1_IDX", 12,  AP_DroneCAN, serial.ports[0].idx, -1),
204

205
    // @Param: S1_BD
206
    // @DisplayName: DroneCAN Serial default baud rate
207
    // @Description: Serial baud rate on remote CAN node
208
    // @CopyFieldsFrom: SERIAL1_BAUD
209
    // @RebootRequired: True
210
    // @User: Advanced
211
    AP_GROUPINFO("S1_BD", 13,  AP_DroneCAN, serial.ports[0].state.baud, 57600),
212

213
    // @Param: S1_PRO
214
    // @DisplayName: Serial protocol of DroneCAN serial port
215
    // @Description: Serial protocol of DroneCAN serial port
216
    // @CopyFieldsFrom: SERIAL1_PROTOCOL
217
    // @RebootRequired: True
218
    // @User: Advanced
219
    AP_GROUPINFO("S1_PRO", 14,  AP_DroneCAN, serial.ports[0].state.protocol, -1),
220

221
#if AP_DRONECAN_SERIAL_NUM_PORTS > 1
222
    // @Param: S2_NOD
223
    // @DisplayName: Serial CAN remote node number
224
    // @Description: CAN remote node number for serial port
225
    // @CopyFieldsFrom: CAN_D1_UC_S1_NOD
226
    AP_GROUPINFO("S2_NOD", 15,  AP_DroneCAN, serial.ports[1].node, 0),
227

228
    // @Param: S2_IDX
229
    // @DisplayName: Serial port number on remote CAN node
230
    // @Description: Serial port number on remote CAN node
231
    // @CopyFieldsFrom: CAN_D1_UC_S1_IDX
232
    AP_GROUPINFO("S2_IDX", 16,  AP_DroneCAN, serial.ports[1].idx, -1),
233

234
    // @Param: S2_BD
235
    // @DisplayName: DroneCAN Serial default baud rate
236
    // @Description: Serial baud rate on remote CAN node
237
    // @CopyFieldsFrom: CAN_D1_UC_S1_BD
238
    AP_GROUPINFO("S2_BD", 17,  AP_DroneCAN, serial.ports[1].state.baud, 57600),
239

240
    // @Param: S2_PRO
241
    // @DisplayName: Serial protocol of DroneCAN serial port
242
    // @Description: Serial protocol of DroneCAN serial port
243
    // @CopyFieldsFrom: CAN_D1_UC_S1_PRO
244
    AP_GROUPINFO("S2_PRO", 18,  AP_DroneCAN, serial.ports[1].state.protocol, -1),
245
#endif
246

247
#if AP_DRONECAN_SERIAL_NUM_PORTS > 2
248
    // @Param: S3_NOD
249
    // @DisplayName: Serial CAN remote node number
250
    // @Description: CAN node number for serial port
251
    // @CopyFieldsFrom: CAN_D1_UC_S1_NOD
252
    AP_GROUPINFO("S3_NOD", 19,  AP_DroneCAN, serial.ports[2].node, 0),
253

254
    // @Param: S3_IDX
255
    // @DisplayName: Serial port number on remote CAN node
256
    // @Description: Serial port number on remote CAN node
257
    // @CopyFieldsFrom: CAN_D1_UC_S1_IDX
258
    AP_GROUPINFO("S3_IDX", 20,  AP_DroneCAN, serial.ports[2].idx, 0),
259

260
    // @Param: S3_BD
261
    // @DisplayName: Serial baud rate on remote CAN node
262
    // @Description: Serial baud rate on remote CAN node
263
    // @CopyFieldsFrom: CAN_D1_UC_S1_BD
264
    AP_GROUPINFO("S3_BD", 21,  AP_DroneCAN, serial.ports[2].state.baud, 57600),
265

266
    // @Param: S3_PRO
267
    // @DisplayName: Serial protocol of DroneCAN serial port
268
    // @Description: Serial protocol of DroneCAN serial port
269
    // @CopyFieldsFrom: CAN_D1_UC_S1_PRO
270
    AP_GROUPINFO("S3_PRO", 22,  AP_DroneCAN, serial.ports[2].state.protocol, -1),
271
#endif
272
#endif // AP_DRONECAN_SERIAL_ENABLED
273

274
    // RLY_RT is index 23 but has to be above SER_EN so its not hidden
275

276
    AP_GROUPEND
277
};
278

279
// this is the timeout in milliseconds for periodic message types. We
280
// set this to 1 to minimise resend of stale msgs
281
#define CAN_PERIODIC_TX_TIMEOUT_MS 2
282

283
AP_DroneCAN::AP_DroneCAN(const int driver_index) :
284
_driver_index(driver_index),
285
canard_iface(driver_index),
286
_dna_server(*this, canard_iface, driver_index)
287
{
288
    AP_Param::setup_object_defaults(this, var_info);
289

290
    for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) {
291
        _SRV_conf[i].esc_pending = false;
292
        _SRV_conf[i].servo_pending = false;
293
    }
294

295
    debug_dronecan(AP_CANManager::LOG_INFO, "AP_DroneCAN constructed\n\r");
296
}
297

298
AP_DroneCAN::~AP_DroneCAN()
299
{
300
}
301

302
AP_DroneCAN *AP_DroneCAN::get_dronecan(uint8_t driver_index)
303
{
304
    if (driver_index >= AP::can().get_num_drivers() ||
305
        AP::can().get_driver_type(driver_index) != AP_CAN::Protocol::DroneCAN) {
306
        return nullptr;
307
    }
308
    return static_cast<AP_DroneCAN*>(AP::can().get_driver(driver_index));
309
}
310

311
bool AP_DroneCAN::add_interface(AP_HAL::CANIface* can_iface)
312
{
313
    if (!canard_iface.add_interface(can_iface)) {
314
        debug_dronecan(AP_CANManager::LOG_ERROR, "DroneCAN: can't add DroneCAN interface\n\r");
315
        return false;   
316
    }
317
    return true;
318
}
319

320
void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters)
321
{
322
    if (driver_index != _driver_index) {
323
        debug_dronecan(AP_CANManager::LOG_ERROR, "DroneCAN: init called with wrong driver_index");
324
        return;
325
    }
326
    if (_initialized) {
327
        debug_dronecan(AP_CANManager::LOG_ERROR, "DroneCAN: init called more than once\n\r");
328
        return;
329
    }
330
    uint8_t node = _dronecan_node;
331
    if (node < 1 || node > 125) { // reset to default if invalid
332
        _dronecan_node.set(AP_DRONECAN_DEFAULT_NODE);
333
        node = AP_DRONECAN_DEFAULT_NODE;
334
    }
335

336
    node_info_rsp.name.len = hal.util->snprintf((char*)node_info_rsp.name.data, sizeof(node_info_rsp.name.data), "org.ardupilot:%u", driver_index);
337

338
    node_info_rsp.software_version.major = AP_DRONECAN_SW_VERS_MAJOR;
339
    node_info_rsp.software_version.minor = AP_DRONECAN_SW_VERS_MINOR;
340
    node_info_rsp.hardware_version.major = AP_DRONECAN_HW_VERS_MAJOR;
341
    node_info_rsp.hardware_version.minor = AP_DRONECAN_HW_VERS_MINOR;
342

343
#if HAL_CANFD_SUPPORTED
344
    if (option_is_set(Options::CANFD_ENABLED)) {
345
        canard_iface.set_canfd(true);
346
    }
347
#endif
348

349
    uint8_t uid_len = sizeof(uavcan_protocol_HardwareVersion::unique_id);
350
    uint8_t unique_id[sizeof(uavcan_protocol_HardwareVersion::unique_id)];
351

352
    mem_pool = NEW_NOTHROW uint32_t[_pool_size/sizeof(uint32_t)];
353
    if (mem_pool == nullptr) {
354
        debug_dronecan(AP_CANManager::LOG_ERROR, "DroneCAN: Failed to allocate memory pool\n\r");
355
        return;
356
    }
357
    canard_iface.init(mem_pool, (_pool_size/sizeof(uint32_t))*sizeof(uint32_t), node);
358

359
    if (!hal.util->get_system_id_unformatted(unique_id, uid_len)) {
360
        return;
361
    }
362
    unique_id[uid_len - 1] += node;
363
    memcpy(node_info_rsp.hardware_version.unique_id, unique_id, uid_len);
364

365
    //Start Servers
366
    if (!_dna_server.init(unique_id, uid_len, node)) {
367
        debug_dronecan(AP_CANManager::LOG_ERROR, "DroneCAN: Failed to start DNA Server\n\r");
368
        return;
369
    }
370

371
    // Roundup all subscribers from supported drivers
372
    bool subscribed = true;
373
#if AP_GPS_DRONECAN_ENABLED
374
    subscribed = subscribed && AP_GPS_DroneCAN::subscribe_msgs(this);
375
#endif
376
#if AP_COMPASS_DRONECAN_ENABLED
377
    subscribed = subscribed && AP_Compass_DroneCAN::subscribe_msgs(this);
378
#endif
379
#if AP_BARO_DRONECAN_ENABLED
380
    subscribed = subscribed && AP_Baro_DroneCAN::subscribe_msgs(this);
381
#endif
382
    subscribed = subscribed && AP_BattMonitor_DroneCAN::subscribe_msgs(this);
383
#if AP_AIRSPEED_DRONECAN_ENABLED
384
    subscribed = subscribed && AP_Airspeed_DroneCAN::subscribe_msgs(this);
385
#endif
386
#if AP_OPTICALFLOW_HEREFLOW_ENABLED
387
    subscribed = subscribed && AP_OpticalFlow_HereFlow::subscribe_msgs(this);
388
#endif
389
#if AP_RANGEFINDER_DRONECAN_ENABLED
390
    subscribed = subscribed && AP_RangeFinder_DroneCAN::subscribe_msgs(this);
391
#endif
392
#if AP_RCPROTOCOL_DRONECAN_ENABLED
393
    subscribed = subscribed && AP_RCProtocol_DroneCAN::subscribe_msgs(this);
394
#endif
395
#if AP_EFI_DRONECAN_ENABLED
396
    subscribed = subscribed && AP_EFI_DroneCAN::subscribe_msgs(this);
397
#endif
398

399
#if AP_PROXIMITY_DRONECAN_ENABLED
400
    subscribed = subscribed && AP_Proximity_DroneCAN::subscribe_msgs(this);
401
#endif
402
#if HAL_MOUNT_XACTI_ENABLED
403
    subscribed = subscribed && AP_Mount_Xacti::subscribe_msgs(this);
404
#endif
405
#if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED
406
    subscribed = subscribed && AP_TemperatureSensor_DroneCAN::subscribe_msgs(this);
407
#endif
408
#if AP_RPM_DRONECAN_ENABLED
409
    subscribed = subscribed && AP_RPM_DroneCAN::subscribe_msgs(this);
410
#endif
411

412
    if (!subscribed) {
413
        AP_BoardConfig::allocation_error("DroneCAN callback");
414
    }
415

416
    act_out_array.set_timeout_ms(5);
417
    act_out_array.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
418

419
    esc_raw.set_timeout_ms(2);
420
    // esc_raw is one higher than high priority to ensure that it is given higher priority over act_out_array
421
    esc_raw.set_priority(CANARD_TRANSFER_PRIORITY_HIGH - 1);
422

423
#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
424
    esc_hobbywing_raw.set_timeout_ms(2);
425
    esc_hobbywing_raw.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
426
#endif
427

428
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
429
    himark_out.set_timeout_ms(2);
430
    himark_out.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
431
#endif
432

433
    rgb_led.set_timeout_ms(20);
434
    rgb_led.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
435

436
    buzzer.set_timeout_ms(20);
437
    buzzer.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
438

439
    safety_state.set_timeout_ms(20);
440
    safety_state.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
441

442
    arming_status.set_timeout_ms(20);
443
    arming_status.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
444

445
#if AP_DRONECAN_SEND_GPS
446
    gnss_fix2.set_timeout_ms(20);
447
    gnss_fix2.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
448

449
    gnss_auxiliary.set_timeout_ms(20);
450
    gnss_auxiliary.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
451

452
    gnss_heading.set_timeout_ms(20);
453
    gnss_heading.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
454

455
    gnss_status.set_timeout_ms(20);
456
    gnss_status.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
457
#endif
458

459
    rtcm_stream.set_timeout_ms(20);
460
    rtcm_stream.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
461

462
    notify_state.set_timeout_ms(20);
463
    notify_state.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
464

465
#if HAL_MOUNT_XACTI_ENABLED
466
    xacti_copter_att_status.set_timeout_ms(20);
467
    xacti_copter_att_status.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
468
    xacti_gimbal_control_data.set_timeout_ms(20);
469
    xacti_gimbal_control_data.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
470
    xacti_gnss_status.set_timeout_ms(20);
471
    xacti_gnss_status.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
472
#endif
473

474
#if AP_RELAY_DRONECAN_ENABLED
475
    relay_hardpoint.set_timeout_ms(20);
476
    relay_hardpoint.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
477
#endif
478

479
    param_save_client.set_timeout_ms(20);
480
    param_save_client.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
481

482
    param_get_set_client.set_timeout_ms(20);
483
    param_get_set_client.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
484

485
    node_status.set_priority(CANARD_TRANSFER_PRIORITY_LOWEST);
486
    node_status.set_timeout_ms(1000);
487

488
    protocol_stats.set_priority(CANARD_TRANSFER_PRIORITY_LOWEST);
489
    protocol_stats.set_timeout_ms(3000);
490

491
    can_stats.set_priority(CANARD_TRANSFER_PRIORITY_LOWEST);
492
    can_stats.set_timeout_ms(3000);
493

494
    node_info_server.set_timeout_ms(20);
495

496
    // setup node status
497
    node_status_msg.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;
498
    node_status_msg.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
499
    node_status_msg.sub_mode = 0;
500

501
    // Spin node for device discovery
502
    for (uint8_t i = 0; i < 5; i++) {
503
        send_node_status();
504
        canard_iface.process(1000);
505
    }
506

507
    hal.util->snprintf(_thread_name, sizeof(_thread_name), "dronecan_%u", driver_index);
508

509
    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_DroneCAN::loop, void), _thread_name, DRONECAN_STACK_SIZE, AP_HAL::Scheduler::PRIORITY_CAN, 0)) {
510
        debug_dronecan(AP_CANManager::LOG_ERROR, "DroneCAN: couldn't create thread\n\r");
511
        return;
512
    }
513

514
#if AP_DRONECAN_SERIAL_ENABLED
515
    serial.init(this);
516
#endif
517

518
    _initialized = true;
519
    debug_dronecan(AP_CANManager::LOG_INFO, "DroneCAN: init done\n\r");
520
}
521

522
void AP_DroneCAN::loop(void)
523
{
524
    while (true) {
525
        if (!_initialized) {
526
            hal.scheduler->delay_microseconds(1000);
527
            continue;
528
        }
529

530
        // ensure that the DroneCAN thread cannot completely saturate
531
        // the CPU, preventing low priority threads from running
532
        hal.scheduler->delay_microseconds(100);
533

534
        canard_iface.process(1);
535

536
        safety_state_send();
537
        notify_state_send();
538
        check_parameter_callback_timeout();
539
        send_parameter_request();
540
        send_parameter_save_request();
541
        send_node_status();
542
        _dna_server.verify_nodes();
543

544
#if AP_DRONECAN_SEND_GPS && AP_GPS_DRONECAN_ENABLED
545
        if (option_is_set(AP_DroneCAN::Options::SEND_GNSS) && !AP_GPS_DroneCAN::instance_exists(this)) {
546
            // send if enabled and this interface/driver is not used by the AP_GPS driver
547
            gnss_send_fix();
548
            gnss_send_yaw();
549
        }
550
#endif
551
        logging();
552
#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
553
        hobbywing_ESC_update();
554
#endif
555
        if (_SRV_armed_mask != 0) {
556
            // we have active servos
557
            uint32_t now = AP_HAL::micros();
558
            const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get());
559
            if (now - _SRV_last_send_us >= servo_period_us) {
560
                _SRV_last_send_us = now;
561
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
562
                if (option_is_set(Options::USE_HIMARK_SERVO)) {
563
                    SRV_send_himark();
564
                } else
565
#endif
566
                {
567
                    SRV_send_actuator();
568
                }
569
                for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) {
570
                    _SRV_conf[i].servo_pending = false;
571
                }
572
            }
573
        }
574

575
#if AP_DRONECAN_SERIAL_ENABLED
576
        serial.update();
577
#endif
578

579
#if AP_RELAY_DRONECAN_ENABLED
580
        relay_hardpoint_send();
581
#endif
582
    }
583
}
584

585
#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
586
void AP_DroneCAN::hobbywing_ESC_update(void)
587
{
588
    if (hal.util->get_soft_armed()) {
589
        // don't update ID database while disarmed, as it can cause
590
        // some hobbywing ESCs to stutter
591
        return;
592
    }
593
    uint32_t now = AP_HAL::millis();
594
    if (now - hobbywing.last_GetId_send_ms >= 1000U) {
595
        hobbywing.last_GetId_send_ms = now;
596
        com_hobbywing_esc_GetEscID msg;
597
        msg.payload.len = 1;
598
        msg.payload.data[0] = 0;
599
        esc_hobbywing_GetEscID.broadcast(msg);
600
    }
601
}
602

603
/*
604
  handle hobbywing GetEscID reply. This gets us the mapping between CAN NodeID and throttle channel
605
 */
606
void AP_DroneCAN::handle_hobbywing_GetEscID(const CanardRxTransfer& transfer, const com_hobbywing_esc_GetEscID& msg)
607
{
608
    if (msg.payload.len == 2 &&
609
        msg.payload.data[0] == transfer.source_node_id) {
610
        // throttle channel is 2nd payload byte
611
        const uint8_t thr_channel = msg.payload.data[1];
612
        if (thr_channel > 0 && thr_channel <= HOBBYWING_MAX_ESC) {
613
            hobbywing.thr_chan[thr_channel-1] = transfer.source_node_id;
614
        }
615
    }
616
}
617

618
/*
619
  find the ESC index given a CAN node ID
620
 */
621
bool AP_DroneCAN::hobbywing_find_esc_index(uint8_t node_id, uint8_t &esc_index) const
622
{
623
    for (uint8_t i=0; i<HOBBYWING_MAX_ESC; i++) {
624
        if (hobbywing.thr_chan[i] == node_id) {
625
            const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
626
            esc_index = i + esc_offset;
627
            return true;
628
        }
629
    }
630
    return false;
631
}
632

633
/*
634
  handle hobbywing StatusMsg1 reply
635
 */
636
void AP_DroneCAN::handle_hobbywing_StatusMsg1(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg1& msg)
637
{
638
    uint8_t esc_index;
639
    if (hobbywing_find_esc_index(transfer.source_node_id, esc_index)) {
640
        update_rpm(esc_index, msg.rpm);
641
    }
642
}
643

644
/*
645
  handle hobbywing StatusMsg2 reply
646
 */
647
void AP_DroneCAN::handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg2& msg)
648
{
649
    uint8_t esc_index;
650
    if (hobbywing_find_esc_index(transfer.source_node_id, esc_index)) {
651
        TelemetryData t {
652
            .temperature_cdeg = int16_t(msg.temperature*100),
653
            .voltage = msg.input_voltage*0.1f,
654
            .current = msg.current*0.1f,
655
        };
656
        update_telem_data(esc_index, t,
657
                          AP_ESC_Telem_Backend::TelemetryType::CURRENT|
658
                          AP_ESC_Telem_Backend::TelemetryType::VOLTAGE|
659
                          AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
660
    }
661

662
}
663
#endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT
664

665
void AP_DroneCAN::send_node_status(void)
666
{
667
    const uint32_t now = AP_HAL::millis();
668
    if (now - _node_status_last_send_ms < 1000) {
669
        return;
670
    }
671
    _node_status_last_send_ms = now;
672
    node_status_msg.uptime_sec = now / 1000;
673
    node_status.broadcast(node_status_msg);
674

675
    if (option_is_set(Options::ENABLE_STATS)) {
676
        // also send protocol and can stats
677
        protocol_stats.broadcast(canard_iface.protocol_stats);
678

679
        // get can stats
680
        for (uint8_t i=0; i<canard_iface.num_ifaces; i++) {
681
            if (canard_iface.ifaces[i] == nullptr) {
682
                continue;
683
            }
684
            auto* iface = hal.can[0]; 
685
            for (uint8_t j=0; j<HAL_NUM_CAN_IFACES; j++) {
686
                if (hal.can[j] == canard_iface.ifaces[i]) {
687
                    iface = hal.can[j];
688
                    break;
689
                }
690
            }
691
            auto* bus_stats = iface->get_statistics();
692
            if (bus_stats == nullptr) {
693
                continue;
694
            }
695
            dronecan_protocol_CanStats can_stats_msg;
696
            can_stats_msg.interface = i;
697
            can_stats_msg.tx_requests = bus_stats->tx_requests;
698
            can_stats_msg.tx_rejected = bus_stats->tx_rejected;
699
            can_stats_msg.tx_overflow = bus_stats->tx_overflow;
700
            can_stats_msg.tx_success = bus_stats->tx_success;
701
            can_stats_msg.tx_timedout = bus_stats->tx_timedout;
702
            can_stats_msg.tx_abort = bus_stats->tx_abort;
703
            can_stats_msg.rx_received = bus_stats->rx_received;
704
            can_stats_msg.rx_overflow = bus_stats->rx_overflow;
705
            can_stats_msg.rx_errors = bus_stats->rx_errors;
706
            can_stats_msg.busoff_errors = bus_stats->num_busoff_err;
707
            can_stats.broadcast(can_stats_msg);
708
        }
709
    }
710
}
711

712
void AP_DroneCAN::handle_node_info_request(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoRequest& req)
713
{
714
    node_info_rsp.status = node_status_msg;
715
    node_info_rsp.status.uptime_sec = AP_HAL::millis() / 1000;
716

717
    node_info_server.respond(transfer, node_info_rsp);
718
}
719

720
int16_t AP_DroneCAN::scale_esc_output(uint8_t idx){
721
    static const int16_t cmd_max = ((1<<13)-1);
722
    float scaled = hal.rcout->scale_esc_to_unity(_SRV_conf[idx].pulse);
723
    // Prevent invalid values (from misconfigured scaling parameters) from sending non-zero commands
724
    if (!isfinite(scaled)) {
725
        return 0;
726
    }
727
    scaled = constrain_float(scaled, -1, 1);
728
    //Check if this channel has a reversible ESC. If it does, we can send negative commands.
729
    if ((((uint32_t) 1) << idx) & _esc_rv) {
730
        scaled *= cmd_max;
731
    } else {
732
        scaled = cmd_max * (scaled + 1.0) / 2.0;
733
    }
734

735
    return static_cast<int16_t>(scaled);
736
}
737

738
///// SRV output /////
739

740
void AP_DroneCAN::SRV_send_actuator(void)
741
{
742
    uint8_t starting_servo = 0;
743
    bool repeat_send;
744

745
    WITH_SEMAPHORE(SRV_sem);
746

747
    do {
748
        repeat_send = false;
749
        uavcan_equipment_actuator_ArrayCommand msg;
750

751
        uint8_t i;
752
        // DroneCAN can hold maximum of 15 commands in one frame
753
        for (i = 0; starting_servo < DRONECAN_SRV_NUMBER && i < 15; starting_servo++) {
754
            uavcan_equipment_actuator_Command cmd;
755

756
            /*
757
             * Servo output uses a range of 1000-2000 PWM for scaling.
758
             * This converts output PWM from [1000:2000] range to [-1:1] range that
759
             * is passed to servo as unitless type via DroneCAN.
760
             * This approach allows for MIN/TRIM/MAX values to be used fully on
761
             * autopilot side and for servo it should have the setup to provide maximum
762
             * physically possible throws at [-1:1] limits.
763
             */
764

765
            if (_SRV_conf[starting_servo].servo_pending && ((((uint32_t) 1) << starting_servo) & _SRV_armed_mask)) {
766
                cmd.actuator_id = starting_servo + 1;
767

768
                if (option_is_set(Options::USE_ACTUATOR_PWM)) {
769
                    cmd.command_type = UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_PWM;
770
                    cmd.command_value = _SRV_conf[starting_servo].pulse;
771
                } else {
772
                    cmd.command_type = UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_UNITLESS;
773
                    cmd.command_value = constrain_float(((float) _SRV_conf[starting_servo].pulse - 1000.0) / 500.0 - 1.0, -1.0, 1.0);
774
                }
775

776
                msg.commands.data[i] = cmd;
777

778
                i++;
779
            }
780
        }
781
        msg.commands.len = i;
782
        if (i > 0) {
783
            if (act_out_array.broadcast(msg) > 0) {
784
                _srv_send_count++;
785
            } else {
786
                _fail_send_count++;
787
            }
788

789
            if (i == 15) {
790
                repeat_send = true;
791
            }
792
        }
793
    } while (repeat_send);
794
}
795

796
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
797
/*
798
  Himark servo output. This uses com.himark.servo.ServoCmd packets
799
 */
800
void AP_DroneCAN::SRV_send_himark(void)
801
{
802
    WITH_SEMAPHORE(SRV_sem);
803

804
    // ServoCmd can hold maximum of 17 commands. First find the highest pending servo < 17
805
    int8_t highest_to_send = -1;
806
    for (int8_t i = 16; i >= 0; i--) {
807
        if (_SRV_conf[i].servo_pending && ((1U<<i) & _SRV_armed_mask) != 0) {
808
            highest_to_send = i;
809
            break;
810
        }
811
    }
812
    if (highest_to_send == -1) {
813
        // nothing to send
814
        return;
815
    }
816
    com_himark_servo_ServoCmd msg {};
817

818
    for (uint8_t i = 0; i <= highest_to_send; i++) {
819
        if ((1U<<i) & _SRV_armed_mask) {
820
            const uint16_t pulse = constrain_int16(_SRV_conf[i].pulse - 1000, 0, 1000);
821
            msg.cmd.data[i] = pulse;
822
        }
823
    }
824
    msg.cmd.len = highest_to_send+1;
825

826
    himark_out.broadcast(msg);
827
}
828
#endif // AP_DRONECAN_HIMARK_SERVO_SUPPORT
829

830
void AP_DroneCAN::SRV_send_esc(void)
831
{
832
    uavcan_equipment_esc_RawCommand esc_msg;
833

834
    uint8_t active_esc_num = 0, max_esc_num = 0;
835
    uint8_t k = 0;
836

837
    // esc offset allows for efficient packing of higher ESC numbers in RawCommand
838
    const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
839

840
    // find out how many esc we have enabled and if they are active at all
841
    for (uint8_t i = esc_offset; i < DRONECAN_SRV_NUMBER; i++) {
842
        if ((((uint32_t) 1) << i) & _ESC_armed_mask) {
843
            max_esc_num = i + 1;
844
            if (_SRV_conf[i].esc_pending) {
845
                active_esc_num++;
846
            }
847
        }
848
    }
849

850
    // if at least one is active (update) we need to send to all
851
    if (active_esc_num > 0) {
852
        k = 0;
853
        const bool armed = hal.util->get_soft_armed();
854
        for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) {
855
            if (armed && ((((uint32_t) 1U) << i) & _ESC_armed_mask)) {
856
                esc_msg.cmd.data[k] = scale_esc_output(i);
857
            } else {
858
                esc_msg.cmd.data[k] = static_cast<unsigned>(0);
859
            }
860

861
            k++;
862
        }
863
        esc_msg.cmd.len = k;
864

865
        if (esc_raw.broadcast(esc_msg)) {
866
            _esc_send_count++;
867
        } else {
868
            _fail_send_count++;
869
        }
870
        // immediately push data to CAN bus
871
        canard_iface.processTx(true);
872
    }
873

874
    for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) {
875
        _SRV_conf[i].esc_pending = false;
876
    }
877
}
878

879
#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
880
/*
881
  support for Hobbywing DroneCAN ESCs
882
 */
883
void AP_DroneCAN::SRV_send_esc_hobbywing(void)
884
{
885
    com_hobbywing_esc_RawCommand esc_msg;
886

887
    uint8_t active_esc_num = 0, max_esc_num = 0;
888
    uint8_t k = 0;
889

890
    // esc offset allows for efficient packing of higher ESC numbers in RawCommand
891
    const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
892

893
    // find out how many esc we have enabled and if they are active at all
894
    for (uint8_t i = esc_offset; i < DRONECAN_SRV_NUMBER; i++) {
895
        if ((((uint32_t) 1) << i) & _ESC_armed_mask) {
896
            max_esc_num = i + 1;
897
            if (_SRV_conf[i].esc_pending) {
898
                active_esc_num++;
899
            }
900
        }
901
    }
902

903
    // if at least one is active (update) we need to send to all
904
    if (active_esc_num > 0) {
905
        k = 0;
906
        const bool armed = hal.util->get_soft_armed();
907
        for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) {
908
            if (armed && ((((uint32_t) 1U) << i) & _ESC_armed_mask)) {
909
                esc_msg.command.data[k] = scale_esc_output(i);
910
            } else {
911
                esc_msg.command.data[k] = static_cast<unsigned>(0);
912
            }
913

914
            k++;
915
        }
916
        esc_msg.command.len = k;
917

918
        if (esc_hobbywing_raw.broadcast(esc_msg)) {
919
            _esc_send_count++;
920
        } else {
921
            _fail_send_count++;
922
        }
923
        // immediately push data to CAN bus
924
        canard_iface.processTx(true);
925
    }
926
}
927
#endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT
928

929
void AP_DroneCAN::SRV_push_servos()
930
{
931
    WITH_SEMAPHORE(SRV_sem);
932

933
    for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) {
934
        // Check if this channels has any function assigned
935
        if (SRV_Channels::channel_function(i) >= SRV_Channel::k_none) {
936
            _SRV_conf[i].pulse = SRV_Channels::srv_channel(i)->get_output_pwm();
937
            _SRV_conf[i].esc_pending = true;
938
            _SRV_conf[i].servo_pending = true;
939
        }
940
    }
941

942
    uint32_t servo_armed_mask = _servo_bm;
943
    uint32_t esc_armed_mask = _esc_bm;
944
    const bool safety_off = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
945
    if (!safety_off) {
946
        AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
947
        if (boardconfig != nullptr) {
948
            const uint32_t safety_mask = boardconfig->get_safety_mask();
949
            servo_armed_mask &= safety_mask;
950
            esc_armed_mask &= safety_mask;
951
        } else {
952
            servo_armed_mask = 0;
953
            esc_armed_mask = 0;
954
        }
955
    }
956
    _SRV_armed_mask = servo_armed_mask;
957
    _ESC_armed_mask = esc_armed_mask;
958

959
    if (_ESC_armed_mask != 0) {
960
        // push ESCs as fast as we can
961
#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
962
        if (option_is_set(Options::USE_HOBBYWING_ESC)) {
963
            SRV_send_esc_hobbywing();
964
        } else
965
#endif
966
        {
967
            SRV_send_esc();
968
        }
969
    }
970
}
971

972
// notify state send
973
void AP_DroneCAN::notify_state_send()
974
{
975
    uint32_t now = AP_HAL::millis();
976

977
    if (_notify_state_hz == 0 || (now - _last_notify_state_ms) < uint32_t(1000 / _notify_state_hz)) {
978
        return;
979
    }
980

981
    ardupilot_indication_NotifyState msg;
982
    msg.vehicle_state = 0;
983
    if (AP_Notify::flags.initialising) {
984
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_INITIALISING;
985
    }
986
    if (AP_Notify::flags.armed) {
987
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_ARMED;
988
    }
989
    if (AP_Notify::flags.flying) {
990
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FLYING;
991
    }
992
    if (AP_Notify::flags.compass_cal_running) {
993
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_MAGCAL_RUN;
994
    }
995
    if (AP_Notify::flags.ekf_bad) {
996
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_EKF_BAD;
997
    }
998
    if (AP_Notify::flags.esc_calibration) {
999
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_ESC_CALIBRATION;
1000
    }
1001
    if (AP_Notify::flags.failsafe_battery) {
1002
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FAILSAFE_BATT;
1003
    }
1004
    if (AP_Notify::flags.failsafe_gcs) {
1005
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FAILSAFE_GCS;
1006
    }
1007
    if (AP_Notify::flags.failsafe_radio) {
1008
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FAILSAFE_RADIO;
1009
    }
1010
    if (AP_Notify::flags.firmware_update) {
1011
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FW_UPDATE;
1012
    }
1013
    if (AP_Notify::flags.gps_fusion) {
1014
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_GPS_FUSION;
1015
    }
1016
    if (AP_Notify::flags.gps_glitching) {
1017
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_GPS_GLITCH;
1018
    }
1019
    if (AP_Notify::flags.have_pos_abs) {
1020
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_POS_ABS_AVAIL;
1021
    }
1022
    if (AP_Notify::flags.leak_detected) {
1023
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_LEAK_DET;
1024
    }
1025
    if (AP_Notify::flags.parachute_release) {
1026
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_CHUTE_RELEASED;
1027
    }
1028
    if (AP_Notify::flags.powering_off) {
1029
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_POWERING_OFF;
1030
    }
1031
    if (AP_Notify::flags.pre_arm_check) {
1032
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_PREARM;
1033
    }
1034
    if (AP_Notify::flags.pre_arm_gps_check) {
1035
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_PREARM_GPS;
1036
    }
1037
    if (AP_Notify::flags.save_trim) {
1038
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_SAVE_TRIM;
1039
    }
1040
    if (AP_Notify::flags.vehicle_lost) {
1041
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_LOST;
1042
    }
1043
    if (AP_Notify::flags.video_recording) {
1044
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_VIDEO_RECORDING;
1045
    }
1046
    if (AP_Notify::flags.waiting_for_throw) {
1047
        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_THROW_READY;
1048
    }
1049

1050
#ifndef HAL_BUILD_AP_PERIPH
1051
    const AP_Vehicle* vehicle = AP::vehicle();
1052
    if (vehicle != nullptr) {
1053
        if (vehicle->is_landing()) {
1054
            msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_IS_LANDING;
1055
        }
1056
        if (vehicle->is_taking_off()) {
1057
            msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_IS_TAKING_OFF;
1058
        }
1059
    }
1060
#endif // HAL_BUILD_AP_PERIPH
1061

1062
    msg.aux_data_type = ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_YAW_EARTH_CENTIDEGREES;
1063
    uint16_t yaw_cd = (uint16_t)(360.0f - degrees(AP::ahrs().get_yaw()))*100.0f;
1064
    const uint8_t *data = (uint8_t *)&yaw_cd;
1065
    for (uint8_t i=0; i<2; i++) {
1066
        msg.aux_data.data[i] = data[i];
1067
    }
1068
    msg.aux_data.len = 2;
1069
    notify_state.broadcast(msg);
1070
    _last_notify_state_ms = AP_HAL::millis();
1071
}
1072

1073
#if AP_DRONECAN_SEND_GPS
1074
void AP_DroneCAN::gnss_send_fix()
1075
{
1076
    const AP_GPS &gps = AP::gps();
1077

1078
    const uint32_t gps_lib_time_ms = gps.last_message_time_ms();
1079
    if (_gnss.last_gps_lib_fix_ms == gps_lib_time_ms) {
1080
        return;
1081
    }
1082
    _gnss.last_gps_lib_fix_ms = gps_lib_time_ms;
1083

1084
    /*
1085
        send Fix2 packet
1086
    */
1087

1088
    uavcan_equipment_gnss_Fix2 pkt {};
1089
    const Location &loc = gps.location();
1090
    const Vector3f &vel = gps.velocity();
1091

1092
    pkt.timestamp.usec = AP_HAL::micros64();
1093
    pkt.gnss_timestamp.usec = gps.time_epoch_usec();
1094
    if (pkt.gnss_timestamp.usec == 0) {
1095
        pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_NONE;
1096
    } else {
1097
        pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_UTC;
1098
    }
1099
    pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL;
1100
    pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL;
1101
    pkt.height_ellipsoid_mm = loc.alt * 10;
1102
    pkt.height_msl_mm = loc.alt * 10;
1103
    for (uint8_t i=0; i<3; i++) {
1104
        pkt.ned_velocity[i] = vel[i];
1105
    }
1106
    pkt.sats_used = gps.num_sats();
1107
    switch (gps.status()) {
1108
    case AP_GPS::GPS_Status::NO_GPS:
1109
    case AP_GPS::GPS_Status::NO_FIX:
1110
        pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_NO_FIX;
1111
        pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE;
1112
        pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER;
1113
        break;
1114
    case AP_GPS::GPS_Status::GPS_OK_FIX_2D:
1115
        pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_2D_FIX;
1116
        pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE;
1117
        pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER;
1118
        break;
1119
    case AP_GPS::GPS_Status::GPS_OK_FIX_3D:
1120
        pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
1121
        pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE;
1122
        pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER;
1123
        break;
1124
    case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS:
1125
        pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
1126
        pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_DGPS;
1127
        pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_SBAS;
1128
        break;
1129
    case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT:
1130
        pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
1131
        pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK;
1132
        pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FLOAT;
1133
        break;
1134
    case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED:
1135
        pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
1136
        pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK;
1137
        pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FIXED;
1138
        break;
1139
    }
1140

1141
    pkt.covariance.len = 6;
1142
    float hacc;
1143
    if (gps.horizontal_accuracy(hacc)) {
1144
        pkt.covariance.data[0] = pkt.covariance.data[1] = sq(hacc);
1145
    }
1146
    float vacc;
1147
    if (gps.vertical_accuracy(vacc)) {
1148
        pkt.covariance.data[2] = sq(vacc);
1149
    }
1150
    float sacc;
1151
    if (gps.speed_accuracy(sacc)) {
1152
        const float vc3 = sq(sacc);
1153
        pkt.covariance.data[3] = pkt.covariance.data[4] = pkt.covariance.data[5] = vc3;
1154
    }
1155

1156
    gnss_fix2.broadcast(pkt);
1157

1158

1159

1160
    const uint32_t now_ms = AP_HAL::millis();
1161
    if (now_ms - _gnss.last_send_status_ms >= 1000) {
1162
        _gnss.last_send_status_ms = now_ms;
1163

1164
        /*
1165
        send aux packet
1166
        */
1167
        uavcan_equipment_gnss_Auxiliary pkt_auxiliary {};
1168
        pkt_auxiliary.hdop = gps.get_hdop() * 0.01;
1169
        pkt_auxiliary.vdop = gps.get_vdop() * 0.01;
1170

1171
        gnss_auxiliary.broadcast(pkt_auxiliary);
1172

1173

1174
        /*
1175
        send Status packet
1176
        */
1177
        ardupilot_gnss_Status pkt_status {};
1178
        pkt_status.healthy = gps.is_healthy();
1179
        if (gps.logging_present() && gps.logging_enabled() && !gps.logging_failed()) {
1180
            pkt_status.status |= ARDUPILOT_GNSS_STATUS_STATUS_LOGGING;
1181
        }
1182
        uint8_t idx; // unused
1183
        if (pkt_status.healthy && !gps.first_unconfigured_gps(idx)) {
1184
            pkt_status.status |= ARDUPILOT_GNSS_STATUS_STATUS_ARMABLE;
1185
        }
1186

1187
        uint32_t error_codes;
1188
        if (gps.get_error_codes(error_codes)) {
1189
            pkt_status.error_codes = error_codes;
1190
        }
1191

1192
        gnss_status.broadcast(pkt_status);
1193
    }
1194
}
1195

1196
void AP_DroneCAN::gnss_send_yaw()
1197
{
1198
    const AP_GPS &gps = AP::gps();
1199

1200
    if (!gps.have_gps_yaw()) {
1201
        return;
1202
    }
1203

1204
    float yaw_deg, yaw_acc_deg;
1205
    uint32_t yaw_time_ms;
1206
    if (!gps.gps_yaw_deg(yaw_deg, yaw_acc_deg, yaw_time_ms) && yaw_time_ms != _gnss.last_lib_yaw_time_ms) {
1207
        return;
1208
    }
1209

1210
    _gnss.last_lib_yaw_time_ms = yaw_time_ms;
1211

1212
    ardupilot_gnss_Heading pkt_heading {};
1213
    pkt_heading.heading_valid = true;
1214
    pkt_heading.heading_accuracy_valid = is_positive(yaw_acc_deg);
1215
    pkt_heading.heading_rad = radians(yaw_deg);
1216
    pkt_heading.heading_accuracy_rad = radians(yaw_acc_deg);
1217

1218
    gnss_heading.broadcast(pkt_heading);
1219
}
1220
#endif // AP_DRONECAN_SEND_GPS
1221

1222
// SafetyState send
1223
void AP_DroneCAN::safety_state_send()
1224
{
1225
    uint32_t now = AP_HAL::millis();
1226
    if (now - _last_safety_state_ms < 500) {
1227
        // update at 2Hz
1228
        return;
1229
    }
1230
    _last_safety_state_ms = now;
1231

1232
    { // handle SafetyState
1233
        ardupilot_indication_SafetyState safety_msg;
1234
        auto state = hal.util->safety_switch_state();
1235
        if (_SRV_armed_mask != 0 || _ESC_armed_mask != 0) {
1236
            // if we are outputting any servos or ESCs due to
1237
            // BRD_SAFETY_MASK then we need to advertise safety as
1238
            // off, this changes LEDs to indicate unsafe and allows
1239
            // AP_Periph ESCs and servos to run
1240
            state = AP_HAL::Util::SAFETY_ARMED;
1241
        }
1242
        switch (state) {
1243
        case AP_HAL::Util::SAFETY_ARMED:
1244
            safety_msg.status = ARDUPILOT_INDICATION_SAFETYSTATE_STATUS_SAFETY_OFF;
1245
            safety_state.broadcast(safety_msg);
1246
            break;
1247
        case AP_HAL::Util::SAFETY_DISARMED:
1248
            safety_msg.status = ARDUPILOT_INDICATION_SAFETYSTATE_STATUS_SAFETY_ON;
1249
            safety_state.broadcast(safety_msg);
1250
            break;
1251
        default:
1252
            // nothing to send
1253
            break;
1254
        }
1255
    }
1256

1257
    { // handle ArmingStatus
1258
        uavcan_equipment_safety_ArmingStatus arming_msg;
1259
        arming_msg.status = hal.util->get_soft_armed() ? UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_FULLY_ARMED :
1260
                                                      UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_DISARMED;
1261
        arming_status.broadcast(arming_msg);
1262
    }
1263
}
1264

1265
// Send relay outputs with hardpoint msg
1266
#if AP_RELAY_DRONECAN_ENABLED
1267
void AP_DroneCAN::relay_hardpoint_send()
1268
{
1269
    const uint32_t now = AP_HAL::millis();
1270
    if ((_relay.rate_hz == 0) || ((now - _relay.last_send_ms) < uint32_t(1000 / _relay.rate_hz))) {
1271
        // Rate limit per user config
1272
        return;
1273
    }
1274
    _relay.last_send_ms = now;
1275

1276
    AP_Relay *relay = AP::relay();
1277
    if (relay == nullptr) {
1278
        return;
1279
    }
1280

1281
    uavcan_equipment_hardpoint_Command msg {};
1282

1283
    // Relay will populate the next command to send and update the last index
1284
    // This will cycle through each relay in turn
1285
    if (relay->dronecan.populate_next_command(_relay.last_index, msg)) {
1286
        relay_hardpoint.broadcast(msg);
1287
    }
1288

1289
}
1290
#endif // AP_RELAY_DRONECAN_ENABLED
1291

1292
/*
1293
  handle Button message
1294
 */
1295
void AP_DroneCAN::handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg)
1296
{
1297
    switch (msg.button) {
1298
    case ARDUPILOT_INDICATION_BUTTON_BUTTON_SAFETY: {
1299
        AP_BoardConfig *brdconfig = AP_BoardConfig::get_singleton();
1300
        if (brdconfig && brdconfig->safety_button_handle_pressed(msg.press_time)) {
1301
            AP_HAL::Util::safety_state state = hal.util->safety_switch_state();
1302
            if (state == AP_HAL::Util::SAFETY_ARMED) {
1303
                hal.rcout->force_safety_on();
1304
            } else {
1305
                hal.rcout->force_safety_off();
1306
            }
1307
        }
1308
        break;
1309
    }
1310
    }
1311
}
1312

1313
/*
1314
  handle traffic report
1315
 */
1316
void AP_DroneCAN::handle_traffic_report(const CanardRxTransfer& transfer, const ardupilot_equipment_trafficmonitor_TrafficReport& msg)
1317
{
1318
#if HAL_ADSB_ENABLED
1319
    AP_ADSB *adsb = AP::ADSB();
1320
    if (!adsb || !adsb->enabled()) {
1321
        // ADSB not enabled
1322
        return;
1323
    }
1324

1325
    AP_ADSB::adsb_vehicle_t vehicle;
1326
    mavlink_adsb_vehicle_t &pkt = vehicle.info;
1327

1328
    pkt.ICAO_address = msg.icao_address;
1329
    pkt.tslc = msg.tslc;
1330
    pkt.lat = msg.latitude_deg_1e7;
1331
    pkt.lon = msg.longitude_deg_1e7;
1332
    pkt.altitude = msg.alt_m * 1000;
1333
    pkt.heading = degrees(msg.heading) * 100;
1334
    pkt.hor_velocity = norm(msg.velocity[0], msg.velocity[1]) * 100;
1335
    pkt.ver_velocity = -msg.velocity[2] * 100;
1336
    pkt.squawk = msg.squawk;
1337
    for (uint8_t i=0; i<9; i++) {
1338
        pkt.callsign[i] = msg.callsign[i];
1339
    }
1340
    pkt.emitter_type = msg.traffic_type;
1341

1342
    if (msg.alt_type == ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_PRESSURE_AMSL) {
1343
        pkt.flags |= ADSB_FLAGS_VALID_ALTITUDE;
1344
        pkt.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH;
1345
    } else if (msg.alt_type == ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_WGS84) {
1346
        pkt.flags |= ADSB_FLAGS_VALID_ALTITUDE;
1347
        pkt.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC;
1348
    }
1349

1350
    if (msg.lat_lon_valid) {
1351
        pkt.flags |= ADSB_FLAGS_VALID_COORDS;
1352
    }
1353
    if (msg.heading_valid) {
1354
        pkt.flags |= ADSB_FLAGS_VALID_HEADING;
1355
    }
1356
    if (msg.velocity_valid) {
1357
        pkt.flags |= ADSB_FLAGS_VALID_VELOCITY;
1358
    }
1359
    if (msg.callsign_valid) {
1360
        pkt.flags |= ADSB_FLAGS_VALID_CALLSIGN;
1361
    }
1362
    if (msg.ident_valid) {
1363
        pkt.flags |= ADSB_FLAGS_VALID_SQUAWK;
1364
    }
1365
    if (msg.simulated_report) {
1366
        pkt.flags |= ADSB_FLAGS_SIMULATED;
1367
    }
1368
    if (msg.vertical_velocity_valid) {
1369
        pkt.flags |= ADSB_FLAGS_VERTICAL_VELOCITY_VALID;
1370
    }
1371
    if (msg.baro_valid) {
1372
        pkt.flags |= ADSB_FLAGS_BARO_VALID;
1373
    }
1374

1375
    vehicle.last_update_ms = AP_HAL::millis() - (vehicle.info.tslc * 1000);
1376
    adsb->handle_adsb_vehicle(vehicle);
1377
#endif
1378
}
1379

1380
/*
1381
  handle actuator status message
1382
 */
1383
#if AP_SERVO_TELEM_ENABLED
1384
void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg)
1385
{
1386
    AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
1387
    if (servo_telem == nullptr) {
1388
        return;
1389
    }
1390

1391
    const AP_Servo_Telem::TelemetryData telem_data {
1392
        .measured_position = ToDeg(msg.position),
1393
        .force = msg.force,
1394
        .speed = msg.speed,
1395
        .duty_cycle = msg.power_rating_pct,
1396
        .present_types = AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
1397
                         AP_Servo_Telem::TelemetryData::Types::FORCE |
1398
                         AP_Servo_Telem::TelemetryData::Types::SPEED |
1399
                         AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE
1400
    };
1401

1402
    servo_telem->update_telem_data(msg.actuator_id, telem_data);
1403
}
1404
#endif
1405

1406
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED
1407
/*
1408
  handle himark ServoInfo message
1409
 */
1410
void AP_DroneCAN::handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg)
1411
{
1412
    AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
1413
    if (servo_telem == nullptr) {
1414
        return;
1415
    }
1416

1417
    const AP_Servo_Telem::TelemetryData telem_data {
1418
        .command_position = msg.pos_cmd * 0.01,
1419
        .measured_position = msg.pos_sensor * 0.01,
1420
        .voltage = msg.voltage * 0.01,
1421
        .current = msg.current * 0.01,
1422
        .motor_temperature_cdeg = int16_t(((msg.motor_temp * 0.2) - 40) * 100),
1423
        .pcb_temperature_cdeg = int16_t(((msg.pcb_temp * 0.2) - 40) * 100),
1424
        .status_flags = msg.error_status,
1425
        .present_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION |
1426
                         AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
1427
                         AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
1428
                         AP_Servo_Telem::TelemetryData::Types::CURRENT |
1429
                         AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP |
1430
                         AP_Servo_Telem::TelemetryData::Types::PCB_TEMP |
1431
                         AP_Servo_Telem::TelemetryData::Types::STATUS
1432
    };
1433

1434
    servo_telem->update_telem_data(msg.servo_id, telem_data);
1435
}
1436
#endif // AP_DRONECAN_HIMARK_SERVO_SUPPORT
1437

1438
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
1439
void AP_DroneCAN::handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg)
1440
{
1441
    AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
1442
    if (servo_telem == nullptr) {
1443
        return;
1444
    }
1445

1446
    const AP_Servo_Telem::TelemetryData telem_data {
1447
        .measured_position = ToDeg(msg.actual_position),
1448
        .voltage = msg.voltage * 0.2,
1449
        .current = msg.current * 0.025,
1450
        .duty_cycle = uint8_t(msg.motor_pwm * (100.0/255.0)),
1451
        .motor_temperature_cdeg = int16_t((msg.motor_temperature - 50) * 100),
1452
        .present_types = AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
1453
                         AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
1454
                         AP_Servo_Telem::TelemetryData::Types::CURRENT |
1455
                         AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE |
1456
                         AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP
1457
    };
1458

1459
    servo_telem->update_telem_data(msg.actuator_id, telem_data);
1460
}
1461
#endif
1462

1463
/*
1464
  handle ESC status message
1465
 */
1466
void AP_DroneCAN::handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg)
1467
{
1468
#if HAL_WITH_ESC_TELEM
1469
    const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
1470
    const uint8_t esc_index = msg.esc_index + esc_offset;
1471

1472
    if (!is_esc_data_index_valid(esc_index)) {
1473
        return;
1474
    }
1475

1476
    TelemetryData t {
1477
        .temperature_cdeg = int16_t((KELVIN_TO_C(msg.temperature)) * 100),
1478
        .voltage = msg.voltage,
1479
        .current = msg.current,
1480
#if AP_EXTENDED_ESC_TELEM_ENABLED
1481
        .power_percentage = msg.power_rating_pct,
1482
#endif
1483
    };
1484

1485
    update_rpm(esc_index, msg.rpm, msg.error_count);
1486
    update_telem_data(esc_index, t,
1487
        (isnan(msg.current) ? 0 : AP_ESC_Telem_Backend::TelemetryType::CURRENT)
1488
            | (isnan(msg.voltage) ? 0 : AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)
1489
            | (isnan(msg.temperature) ? 0 : AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE)
1490
#if AP_EXTENDED_ESC_TELEM_ENABLED
1491
            | AP_ESC_Telem_Backend::TelemetryType::POWER_PERCENTAGE
1492
#endif
1493
        );
1494
#endif // HAL_WITH_ESC_TELEM
1495
}
1496

1497
#if AP_EXTENDED_ESC_TELEM_ENABLED
1498
/*
1499
  handle Extended ESC status message
1500
 */
1501
void AP_DroneCAN::handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg)
1502
{
1503
    const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
1504
    const uint8_t esc_index = msg.esc_index + esc_offset;
1505

1506
    if (!is_esc_data_index_valid(esc_index)) {
1507
        return;
1508
    }
1509

1510
    TelemetryData telemetryData {
1511
        .motor_temp_cdeg = (int16_t)(msg.motor_temperature_degC * 100),
1512
        .input_duty = msg.input_pct,
1513
        .output_duty = msg.output_pct,
1514
        .flags = msg.status_flags,
1515
    };
1516

1517
    update_telem_data(esc_index, telemetryData,
1518
        AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE
1519
        | AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY
1520
        | AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY
1521
        | AP_ESC_Telem_Backend::TelemetryType::FLAGS);
1522
}
1523
#endif // AP_EXTENDED_ESC_TELEM_ENABLED
1524

1525
bool AP_DroneCAN::is_esc_data_index_valid(const uint8_t index) {
1526
    if (index > DRONECAN_SRV_NUMBER) {
1527
        // printf("DroneCAN: invalid esc index: %d. max index allowed: %d\n\r", index, DRONECAN_SRV_NUMBER);
1528
        return false;
1529
    }
1530
    return true;
1531
}
1532

1533
#if AP_SCRIPTING_ENABLED
1534
/*
1535
  handle FlexDebug message, holding a copy locally for a lua script to access
1536
 */
1537
void AP_DroneCAN::handle_FlexDebug(const CanardRxTransfer& transfer, const dronecan_protocol_FlexDebug &msg)
1538
{
1539
    if (!option_is_set(Options::ENABLE_FLEX_DEBUG)) {
1540
        return;
1541
    }
1542

1543
    // find an existing element in the list
1544
    const uint8_t source_node = transfer.source_node_id;
1545
    for (auto *p = flexDebug_list; p != nullptr; p = p->next) {
1546
        if (p->node_id == source_node && p->msg.id == msg.id) {
1547
            p->msg = msg;
1548
            p->timestamp_us = uint32_t(transfer.timestamp_usec);
1549
            return;
1550
        }
1551
    }
1552

1553
    // new message ID, add to the list. Note that this gets called
1554
    // only from one thread, so no lock needed
1555
    auto *p = NEW_NOTHROW FlexDebug;
1556
    if (p == nullptr) {
1557
        return;
1558
    }
1559
    p->node_id = source_node;
1560
    p->msg = msg;
1561
    p->timestamp_us = uint32_t(transfer.timestamp_usec);
1562
    p->next = flexDebug_list;
1563

1564
    // link into the list
1565
    flexDebug_list = p;
1566
}
1567

1568
/*
1569
  get the last FlexDebug message from a node
1570
 */
1571
bool AP_DroneCAN::get_FlexDebug(uint8_t node_id, uint16_t msg_id, uint32_t &timestamp_us, dronecan_protocol_FlexDebug &msg) const
1572
{
1573
    for (const auto *p = flexDebug_list; p != nullptr; p = p->next) {
1574
        if (p->node_id == node_id && p->msg.id == msg_id) {
1575
            if (timestamp_us == p->timestamp_us) {
1576
                // stale message
1577
                return false;
1578
            }
1579
            timestamp_us = p->timestamp_us;
1580
            msg = p->msg;
1581
            return true;
1582
        }
1583
    }
1584
    return false;
1585
}
1586

1587
#endif // AP_SCRIPTING_ENABLED
1588

1589
/*
1590
  handle LogMessage debug
1591
 */
1592
void AP_DroneCAN::handle_debug(const CanardRxTransfer& transfer, const uavcan_protocol_debug_LogMessage& msg)
1593
{
1594
#if AP_HAVE_GCS_SEND_TEXT
1595
    const auto log_level = AP::can().get_log_level();
1596
    const auto msg_level = msg.level.value;
1597
    bool send_mavlink = false;
1598

1599
    if (log_level != AP_CANManager::LOG_NONE) {
1600
        // log to onboard log and mavlink
1601
        enum MAV_SEVERITY mavlink_level = MAV_SEVERITY_INFO;
1602
        switch (msg_level) {
1603
        case UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_DEBUG:
1604
            mavlink_level = MAV_SEVERITY_DEBUG;
1605
            send_mavlink = uint8_t(log_level) >= uint8_t(AP_CANManager::LogLevel::LOG_DEBUG);
1606
            break;
1607
        case UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_INFO:
1608
            mavlink_level = MAV_SEVERITY_INFO;
1609
            send_mavlink = uint8_t(log_level) >= uint8_t(AP_CANManager::LogLevel::LOG_INFO);
1610
            break;
1611
        case UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_WARNING:
1612
            mavlink_level = MAV_SEVERITY_WARNING;
1613
            send_mavlink = uint8_t(log_level) >= uint8_t(AP_CANManager::LogLevel::LOG_WARNING);
1614
            break;
1615
        default:
1616
            send_mavlink = uint8_t(log_level) >= uint8_t(AP_CANManager::LogLevel::LOG_ERROR);
1617
            mavlink_level = MAV_SEVERITY_ERROR;
1618
            break;
1619
        }
1620
        if (send_mavlink) {
1621
            // when we send as MAVLink it also gets logged locally, so
1622
            // we return to avoid a duplicate
1623
            GCS_SEND_TEXT(mavlink_level, "CAN[%u] %s", transfer.source_node_id, msg.text.data);
1624
            return;
1625
        }
1626
    }
1627
#endif // AP_HAVE_GCS_SEND_TEXT
1628

1629
#if HAL_LOGGING_ENABLED
1630
    // always log locally if we have logging enabled
1631
    AP::logger().Write_MessageF("CAN[%u] %s", transfer.source_node_id, msg.text.data);
1632
#endif
1633
}
1634

1635
/*
1636
 check for parameter get/set response timeout
1637
*/
1638
void AP_DroneCAN::check_parameter_callback_timeout()
1639
{
1640
    WITH_SEMAPHORE(_param_sem);
1641

1642
    // return immediately if not waiting for get/set parameter response
1643
    if (param_request_sent_ms == 0) {
1644
        return;
1645
    }
1646

1647
    const uint32_t now_ms = AP_HAL::millis();
1648
    if (now_ms - param_request_sent_ms > AP_DRONECAN_GETSET_TIMEOUT_MS) {
1649
        param_request_sent_ms = 0;
1650
        param_int_cb = nullptr;
1651
        param_float_cb = nullptr;
1652
        param_string_cb = nullptr;
1653
    }
1654
}
1655

1656
/*
1657
  send any queued request to get/set parameter
1658
  called from loop
1659
*/
1660
void AP_DroneCAN::send_parameter_request()
1661
{
1662
    WITH_SEMAPHORE(_param_sem);
1663
    if (param_request_sent) {
1664
        return;
1665
    }
1666
    param_get_set_client.request(param_request_node_id, param_getset_req);
1667
    param_request_sent = true;
1668
}
1669

1670
/*
1671
  set named float parameter on node
1672
*/
1673
bool AP_DroneCAN::set_parameter_on_node(uint8_t node_id, const char *name, float value, ParamGetSetFloatCb *cb)
1674
{
1675
    WITH_SEMAPHORE(_param_sem);
1676

1677
    // fail if waiting for any previous get/set request
1678
    if (param_int_cb != nullptr ||
1679
        param_float_cb != nullptr ||
1680
        param_string_cb != nullptr) {
1681
        return false;
1682
    }
1683
    param_getset_req.index = 0;
1684
    param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data)-1);
1685
    param_getset_req.value.real_value = value;
1686
    param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE;
1687
    param_float_cb = cb;
1688
    param_request_sent = false;
1689
    param_request_sent_ms = AP_HAL::millis();
1690
    param_request_node_id = node_id;
1691
    return true;
1692
}
1693

1694
/*
1695
  set named integer parameter on node
1696
*/
1697
bool AP_DroneCAN::set_parameter_on_node(uint8_t node_id, const char *name, int32_t value, ParamGetSetIntCb *cb)
1698
{
1699
    WITH_SEMAPHORE(_param_sem);
1700

1701
    // fail if waiting for any previous get/set request
1702
    if (param_int_cb != nullptr ||
1703
        param_float_cb != nullptr ||
1704
        param_string_cb != nullptr) {
1705
        return false;
1706
    }
1707
    param_getset_req.index = 0;
1708
    param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data)-1);
1709
    param_getset_req.value.integer_value = value;
1710
    param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
1711
    param_int_cb = cb;
1712
    param_request_sent = false;
1713
    param_request_sent_ms = AP_HAL::millis();
1714
    param_request_node_id = node_id;
1715
    return true;
1716
}
1717

1718
/*
1719
  set named string parameter on node
1720
*/
1721
bool AP_DroneCAN::set_parameter_on_node(uint8_t node_id, const char *name, const string &value, ParamGetSetStringCb *cb)
1722
{
1723
    WITH_SEMAPHORE(_param_sem);
1724

1725
    // fail if waiting for any previous get/set request
1726
    if (param_int_cb != nullptr ||
1727
        param_float_cb != nullptr ||
1728
        param_string_cb != nullptr) {
1729
        return false;
1730
    }
1731
    param_getset_req.index = 0;
1732
    param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data)-1);
1733
    memcpy(&param_getset_req.value.string_value, (const void*)&value, sizeof(value));
1734
    param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE;
1735
    param_string_cb = cb;
1736
    param_request_sent = false;
1737
    param_request_sent_ms = AP_HAL::millis();
1738
    param_request_node_id = node_id;
1739
    return true;
1740
}
1741

1742
/*
1743
  get named float parameter on node
1744
*/
1745
bool AP_DroneCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetFloatCb *cb)
1746
{
1747
    WITH_SEMAPHORE(_param_sem);
1748

1749
    // fail if waiting for any previous get/set request
1750
    if (param_int_cb != nullptr ||
1751
        param_float_cb != nullptr ||
1752
        param_string_cb != nullptr) {
1753
        return false;
1754
    }
1755
    param_getset_req.index = 0;
1756
    param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data));
1757
    param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY;
1758
    param_float_cb = cb;
1759
    param_request_sent = false;
1760
    param_request_sent_ms = AP_HAL::millis();
1761
    param_request_node_id = node_id;
1762
    return true;
1763
}
1764

1765
/*
1766
  get named integer parameter on node
1767
*/
1768
bool AP_DroneCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetIntCb *cb)
1769
{
1770
    WITH_SEMAPHORE(_param_sem);
1771

1772
    // fail if waiting for any previous get/set request
1773
    if (param_int_cb != nullptr ||
1774
        param_float_cb != nullptr ||
1775
        param_string_cb != nullptr) {
1776
        return false;
1777
    }
1778
    param_getset_req.index = 0;
1779
    param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data));
1780
    param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY;
1781
    param_int_cb = cb;
1782
    param_request_sent = false;
1783
    param_request_sent_ms = AP_HAL::millis();
1784
    param_request_node_id = node_id;
1785
    return true;
1786
}
1787

1788
/*
1789
  get named string parameter on node
1790
*/
1791
bool AP_DroneCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetStringCb *cb)
1792
{
1793
    WITH_SEMAPHORE(_param_sem);
1794

1795
    // fail if waiting for any previous get/set request
1796
    if (param_int_cb != nullptr ||
1797
        param_float_cb != nullptr ||
1798
        param_string_cb != nullptr) {
1799
        return false;
1800
    }
1801
    param_getset_req.index = 0;
1802
    param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data));
1803
    param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY;
1804
    param_string_cb = cb;
1805
    param_request_sent = false;
1806
    param_request_sent_ms = AP_HAL::millis();
1807
    param_request_node_id = node_id;
1808
    return true;
1809
}
1810

1811
void AP_DroneCAN::handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp)
1812
{
1813
    WITH_SEMAPHORE(_param_sem);
1814
    if (!param_int_cb &&
1815
        !param_float_cb &&
1816
        !param_string_cb) {
1817
        return;
1818
    }
1819
    if ((rsp.value.union_tag == UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE) && param_int_cb) {
1820
        int32_t val = rsp.value.integer_value;
1821
        if ((*param_int_cb)(this, transfer.source_node_id, (const char*)rsp.name.data, val)) {
1822
            // we want the parameter to be set with val
1823
            param_getset_req.index = 0;
1824
            memcpy(param_getset_req.name.data, rsp.name.data, rsp.name.len);
1825
            param_getset_req.value.integer_value = val;
1826
            param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
1827
            param_request_sent = false;
1828
            param_request_sent_ms = AP_HAL::millis();
1829
            param_request_node_id = transfer.source_node_id;
1830
            return;
1831
        }
1832
    } else if ((rsp.value.union_tag == UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE) && param_float_cb) {
1833
        float val = rsp.value.real_value;
1834
        if ((*param_float_cb)(this, transfer.source_node_id, (const char*)rsp.name.data, val)) {
1835
            // we want the parameter to be set with val
1836
            param_getset_req.index = 0;
1837
            memcpy(param_getset_req.name.data, rsp.name.data, rsp.name.len);
1838
            param_getset_req.value.real_value = val;
1839
            param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE;
1840
            param_request_sent = false;
1841
            param_request_sent_ms = AP_HAL::millis();
1842
            param_request_node_id = transfer.source_node_id;
1843
            return;
1844
        }
1845
    } else if ((rsp.value.union_tag == UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE) && param_string_cb) {
1846
        string val;
1847
        memcpy(&val, &rsp.value.string_value, sizeof(val));
1848
        if ((*param_string_cb)(this, transfer.source_node_id, (const char*)rsp.name.data, val)) {
1849
            // we want the parameter to be set with val
1850
            param_getset_req.index = 0;
1851
            memcpy(param_getset_req.name.data, rsp.name.data, rsp.name.len);
1852
            memcpy(&param_getset_req.value.string_value, &val, sizeof(val));
1853
            param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE;
1854
            param_request_sent = false;
1855
            param_request_sent_ms = AP_HAL::millis();
1856
            param_request_node_id = transfer.source_node_id;
1857
            return;
1858
        }
1859
    }
1860

1861
    param_request_sent_ms = 0;
1862
    param_int_cb = nullptr;
1863
    param_float_cb = nullptr;
1864
    param_string_cb = nullptr;
1865
}
1866

1867
void AP_DroneCAN::send_parameter_save_request()
1868
{
1869
    WITH_SEMAPHORE(_param_save_sem);
1870
    if (param_save_request_sent) {
1871
        return;
1872
    }
1873
    param_save_client.request(param_save_request_node_id, param_save_req);
1874
    param_save_request_sent = true;
1875
}
1876

1877
bool AP_DroneCAN::save_parameters_on_node(uint8_t node_id, ParamSaveCb *cb)
1878
{
1879
    WITH_SEMAPHORE(_param_save_sem);
1880
    if (save_param_cb != nullptr) {
1881
        //busy
1882
        return false;
1883
    }
1884

1885
    param_save_req.opcode = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_SAVE;
1886
    param_save_request_sent = false;
1887
    param_save_request_node_id = node_id;
1888
    save_param_cb = cb;
1889
    return true;
1890
}
1891

1892
// handle parameter save request response
1893
void AP_DroneCAN::handle_param_save_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_ExecuteOpcodeResponse& rsp)
1894
{
1895
    WITH_SEMAPHORE(_param_save_sem);
1896
    if (!save_param_cb) {
1897
        return;
1898
    }
1899
    (*save_param_cb)(this, transfer.source_node_id, rsp.ok);
1900
    save_param_cb = nullptr;
1901
}
1902

1903
// Send Reboot command
1904
// Note: Do not call this from outside DroneCAN thread context,
1905
// THIS IS NOT A THREAD SAFE API!
1906
void AP_DroneCAN::send_reboot_request(uint8_t node_id)
1907
{
1908
    uavcan_protocol_RestartNodeRequest request;
1909
    request.magic_number = UAVCAN_PROTOCOL_RESTARTNODE_REQUEST_MAGIC_NUMBER;
1910
    restart_node_client.request(node_id, request);
1911
}
1912

1913
// check if a option is set and if it is then reset it to 0.
1914
// return true if it was set
1915
bool AP_DroneCAN::check_and_reset_option(Options option)
1916
{
1917
    bool ret = option_is_set(option);
1918
    if (ret) {
1919
        _options.set_and_save(int16_t(_options.get() & ~uint16_t(option)));
1920
    }
1921
    return ret;
1922
}
1923

1924
// handle prearm check
1925
bool AP_DroneCAN::prearm_check(char* fail_msg, uint8_t fail_msg_len) const
1926
{
1927
    // forward this to DNA_Server
1928
    return _dna_server.prearm_check(fail_msg, fail_msg_len);
1929
}
1930

1931
/*
1932
  periodic logging
1933
 */
1934
void AP_DroneCAN::logging(void)
1935
{
1936
#if HAL_LOGGING_ENABLED
1937
    const uint32_t now_ms = AP_HAL::millis();
1938
    if (now_ms - last_log_ms < 1000) {
1939
        return;
1940
    }
1941
    last_log_ms = now_ms;
1942
    if (HAL_NUM_CAN_IFACES <= _driver_index) {
1943
        // no interface?
1944
        return;
1945
    }
1946
    const auto *iface = hal.can[_driver_index];
1947
    if (iface == nullptr) {
1948
        return;
1949
    }
1950
    const auto *stats = iface->get_statistics();
1951
    if (stats == nullptr) {
1952
        // statistics not implemented on this interface
1953
        return;
1954
    }
1955
    const auto &s = *stats;
1956

1957
// @LoggerMessage: CANS
1958
// @Description: CAN Bus Statistics
1959
// @Field: TimeUS: Time since system startup
1960
// @Field: I: driver index
1961
// @Field: T: transmit success count
1962
// @Field: Trq: transmit request count
1963
// @Field: Trej: transmit reject count
1964
// @Field: Tov: transmit overflow count
1965
// @Field: Tto: transmit timeout count
1966
// @Field: Tab: transmit abort count
1967
// @Field: R: receive count
1968
// @Field: Rov: receive overflow count
1969
// @Field: Rer: receive error count
1970
// @Field: Bo: bus offset error count
1971
// @Field: Etx: ESC successful send count
1972
// @Field: Stx: Servo successful send count
1973
// @Field: Ftx: ESC/Servo failed-to-send count
1974
    AP::logger().WriteStreaming("CANS",
1975
                                "TimeUS,I,T,Trq,Trej,Tov,Tto,Tab,R,Rov,Rer,Bo,Etx,Stx,Ftx",
1976
                                "s#-------------",
1977
                                "F--------------",
1978
                                "QBIIIIIIIIIIIII",
1979
                                AP_HAL::micros64(),
1980
                                _driver_index,
1981
                                s.tx_success,
1982
                                s.tx_requests,
1983
                                s.tx_rejected,
1984
                                s.tx_overflow,
1985
                                s.tx_timedout,
1986
                                s.tx_abort,
1987
                                s.rx_received,
1988
                                s.rx_overflow,
1989
                                s.rx_errors,
1990
                                s.num_busoff_err,
1991
                                _esc_send_count,
1992
                                _srv_send_count,
1993
                                _fail_send_count);
1994
#endif // HAL_LOGGING_ENABLED
1995
}
1996

1997
// add an 11 bit auxillary driver
1998
bool AP_DroneCAN::add_11bit_driver(CANSensor *sensor)
1999
{
2000
    return canard_iface.add_11bit_driver(sensor);
2001
}
2002

2003
// handler for outgoing frames for auxillary drivers
2004
bool AP_DroneCAN::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us)
2005
{
2006
    if (out_frame.isExtended()) {
2007
        // don't allow extended frames to be sent by auxillary driver
2008
        return false;
2009
    }
2010
    return canard_iface.write_aux_frame(out_frame, timeout_us);
2011
}
2012

2013
#endif // HAL_NUM_CAN_IFACES
2014

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

Product

Resources

Company

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

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