Line data Source code
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 : // Translation of all messages from DroneCAN structures into AP structures is done
91 : // in AP_DroneCAN and not in corresponding drivers.
92 : // The overhead of including definitions of DSDL is very high and it is best to
93 : // concentrate in one place.
94 :
95 : // table of user settable CAN bus parameters
96 : const AP_Param::GroupInfo AP_DroneCAN::var_info[] = {
97 : // @Param: NODE
98 : // @DisplayName: Own node ID
99 : // @Description: DroneCAN node ID used by the driver itself on this network
100 : // @Range: 1 125
101 : // @User: Advanced
102 : AP_GROUPINFO("NODE", 1, AP_DroneCAN, _dronecan_node, AP_DRONECAN_DEFAULT_NODE),
103 :
104 : // @Param: SRV_BM
105 : // @DisplayName: Output channels to be transmitted as servo over DroneCAN
106 : // @Description: Bitmask with one set for channel to be transmitted as a servo command over DroneCAN
107 : // @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
108 :
109 : // @User: Advanced
110 : AP_GROUPINFO("SRV_BM", 2, AP_DroneCAN, _servo_bm, 0),
111 :
112 : // @Param: ESC_BM
113 : // @DisplayName: Output channels to be transmitted as ESC over DroneCAN
114 : // @Description: Bitmask with one set for channel to be transmitted as a ESC command over DroneCAN
115 : // @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
116 : // @User: Advanced
117 : AP_GROUPINFO("ESC_BM", 3, AP_DroneCAN, _esc_bm, 0),
118 :
119 : // @Param: SRV_RT
120 : // @DisplayName: Servo output rate
121 : // @Description: Maximum transmit rate for servo outputs
122 : // @Range: 1 200
123 : // @Units: Hz
124 : // @User: Advanced
125 : AP_GROUPINFO("SRV_RT", 4, AP_DroneCAN, _servo_rate_hz, 50),
126 :
127 : // @Param: OPTION
128 : // @DisplayName: DroneCAN options
129 : // @Description: Option flags
130 : // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS,6:UseHimarkServo,7:HobbyWingESC,8:EnableStats,9:EnableFlexDebug,10:SecondaryAllowExtendedFrames
131 : // @User: Advanced
132 : AP_GROUPINFO("OPTION", 5, AP_DroneCAN, _options, 0),
133 :
134 : // @Param: NTF_RT
135 : // @DisplayName: Notify State rate
136 : // @Description: Maximum transmit rate for Notify State Message
137 : // @Range: 1 200
138 : // @Units: Hz
139 : // @User: Advanced
140 : AP_GROUPINFO("NTF_RT", 6, AP_DroneCAN, _notify_state_hz, 20),
141 :
142 : // @Param: ESC_OF
143 : // @DisplayName: ESC Output channels offset
144 : // @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 outputs 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
145 : // @Range: 0 18
146 : // @User: Advanced
147 : AP_GROUPINFO("ESC_OF", 7, AP_DroneCAN, _esc_offset, 0),
148 :
149 : // @Param: POOL
150 : // @DisplayName: CAN pool size
151 : // @Description: Amount of memory in bytes to allocate for the DroneCAN memory pool. More memory is needed for higher CAN bus loads
152 : // @Range: 1024 16384
153 : // @User: Advanced
154 : AP_GROUPINFO("POOL", 8, AP_DroneCAN, _pool_size, DRONECAN_NODE_POOL_SIZE),
155 :
156 : // @Param: ESC_RV
157 : // @DisplayName: Bitmask for output channels for reversible ESCs over DroneCAN.
158 : // @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.
159 : // @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
160 : // @User: Advanced
161 : AP_GROUPINFO("ESC_RV", 9, AP_DroneCAN, _esc_rv, 0),
162 :
163 : #if AP_RELAY_DRONECAN_ENABLED
164 : // @Param: RLY_RT
165 : // @DisplayName: DroneCAN relay output rate
166 : // @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
167 : // @Range: 0 200
168 : // @Units: Hz
169 : // @User: Advanced
170 : AP_GROUPINFO("RLY_RT", 23, AP_DroneCAN, _relay.rate_hz, 0),
171 : #endif
172 :
173 : #if AP_DRONECAN_SERIAL_ENABLED
174 : /*
175 : due to the parameter tree depth limitation we can't use a sub-table for the serial parameters
176 : */
177 :
178 : // @Param: SER_EN
179 : // @DisplayName: DroneCAN Serial enable
180 : // @Description: Enable DroneCAN virtual serial ports
181 : // @Values: 0:Disabled, 1:Enabled
182 : // @RebootRequired: True
183 : // @User: Advanced
184 : AP_GROUPINFO_FLAGS("SER_EN", 10, AP_DroneCAN, serial.enable, 0, AP_PARAM_FLAG_ENABLE),
185 :
186 : // @Param: S1_NOD
187 : // @DisplayName: Serial CAN remote node number
188 : // @Description: CAN remote node number for serial port
189 : // @Range: 0 127
190 : // @RebootRequired: True
191 : // @User: Advanced
192 : AP_GROUPINFO("S1_NOD", 11, AP_DroneCAN, serial.ports[0].node, 0),
193 :
194 : // @Param: S1_IDX
195 : // @DisplayName: DroneCAN Serial1 index
196 : // @Description: Serial port number on remote CAN node
197 : // @Range: -1 100
198 : // @Values: -1:Disabled,0:Serial0,1:Serial1,2:Serial2,3:Serial3,4:Serial4,5:Serial5,6:Serial6
199 : // @RebootRequired: True
200 : // @User: Advanced
201 : AP_GROUPINFO("S1_IDX", 12, AP_DroneCAN, serial.ports[0].idx, -1),
202 :
203 : // @Param: S1_BD
204 : // @DisplayName: DroneCAN Serial default baud rate
205 : // @Description: Serial baud rate on remote CAN node
206 : // @CopyFieldsFrom: SERIAL1_BAUD
207 : // @RebootRequired: True
208 : // @User: Advanced
209 : AP_GROUPINFO("S1_BD", 13, AP_DroneCAN, serial.ports[0].state.baud, 57600),
210 :
211 : // @Param: S1_PRO
212 : // @DisplayName: Serial protocol of DroneCAN serial port
213 : // @Description: Serial protocol of DroneCAN serial port
214 : // @CopyFieldsFrom: SERIAL1_PROTOCOL
215 : // @RebootRequired: True
216 : // @User: Advanced
217 : AP_GROUPINFO("S1_PRO", 14, AP_DroneCAN, serial.ports[0].state.protocol, -1),
218 :
219 : #if AP_DRONECAN_SERIAL_NUM_PORTS > 1
220 : // @Param: S2_NOD
221 : // @DisplayName: Serial CAN remote node number
222 : // @Description: CAN remote node number for serial port
223 : // @CopyFieldsFrom: CAN_D1_UC_S1_NOD
224 : AP_GROUPINFO("S2_NOD", 15, AP_DroneCAN, serial.ports[1].node, 0),
225 :
226 : // @Param: S2_IDX
227 : // @DisplayName: Serial port number on remote CAN node
228 : // @Description: Serial port number on remote CAN node
229 : // @CopyFieldsFrom: CAN_D1_UC_S1_IDX
230 : AP_GROUPINFO("S2_IDX", 16, AP_DroneCAN, serial.ports[1].idx, -1),
231 :
232 : // @Param: S2_BD
233 : // @DisplayName: DroneCAN Serial default baud rate
234 : // @Description: Serial baud rate on remote CAN node
235 : // @CopyFieldsFrom: CAN_D1_UC_S1_BD
236 : AP_GROUPINFO("S2_BD", 17, AP_DroneCAN, serial.ports[1].state.baud, 57600),
237 :
238 : // @Param: S2_PRO
239 : // @DisplayName: Serial protocol of DroneCAN serial port
240 : // @Description: Serial protocol of DroneCAN serial port
241 : // @CopyFieldsFrom: CAN_D1_UC_S1_PRO
242 : AP_GROUPINFO("S2_PRO", 18, AP_DroneCAN, serial.ports[1].state.protocol, -1),
243 : #endif
244 :
245 : #if AP_DRONECAN_SERIAL_NUM_PORTS > 2
246 : // @Param: S3_NOD
247 : // @DisplayName: Serial CAN remote node number
248 : // @Description: CAN node number for serial port
249 : // @CopyFieldsFrom: CAN_D1_UC_S1_NOD
250 : AP_GROUPINFO("S3_NOD", 19, AP_DroneCAN, serial.ports[2].node, 0),
251 :
252 : // @Param: S3_IDX
253 : // @DisplayName: Serial port number on remote CAN node
254 : // @Description: Serial port number on remote CAN node
255 : // @CopyFieldsFrom: CAN_D1_UC_S1_IDX
256 : AP_GROUPINFO("S3_IDX", 20, AP_DroneCAN, serial.ports[2].idx, 0),
257 :
258 : // @Param: S3_BD
259 : // @DisplayName: Serial baud rate on remote CAN node
260 : // @Description: Serial baud rate on remote CAN node
261 : // @CopyFieldsFrom: CAN_D1_UC_S1_BD
262 : AP_GROUPINFO("S3_BD", 21, AP_DroneCAN, serial.ports[2].state.baud, 57600),
263 :
264 : // @Param: S3_PRO
265 : // @DisplayName: Serial protocol of DroneCAN serial port
266 : // @Description: Serial protocol of DroneCAN serial port
267 : // @CopyFieldsFrom: CAN_D1_UC_S1_PRO
268 : AP_GROUPINFO("S3_PRO", 22, AP_DroneCAN, serial.ports[2].state.protocol, -1),
269 : #endif
270 : #endif // AP_DRONECAN_SERIAL_ENABLED
271 :
272 : // RLY_RT is index 23 but has to be above SER_EN so its not hidden
273 :
274 : AP_GROUPEND
275 : };
276 :
277 : // this is the timeout in milliseconds for periodic message types. We
278 : // set this to 1 to minimise resend of stale msgs
279 : #define CAN_PERIODIC_TX_TIMEOUT_MS 2
280 :
281 5 : AP_DroneCAN::AP_DroneCAN(const int driver_index) :
282 5 : _driver_index(driver_index),
283 5 : canard_iface(driver_index),
284 10 : _dna_server(*this, canard_iface, driver_index)
285 : {
286 5 : AP_Param::setup_object_defaults(this, var_info);
287 :
288 165 : for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) {
289 160 : _SRV_conf[i].esc_pending = false;
290 160 : _SRV_conf[i].servo_pending = false;
291 : }
292 5 : }
293 :
294 0 : AP_DroneCAN::~AP_DroneCAN()
295 : {
296 0 : }
297 :
298 157816 : AP_DroneCAN *AP_DroneCAN::get_dronecan(uint8_t driver_index)
299 : {
300 315632 : if (driver_index >= AP::can().get_num_drivers() ||
301 157816 : AP::can().get_driver_type(driver_index) != AP_CAN::Protocol::DroneCAN) {
302 0 : return nullptr;
303 : }
304 157816 : return static_cast<AP_DroneCAN*>(AP::can().get_driver(driver_index));
305 : }
306 :
307 5 : bool AP_DroneCAN::add_interface(AP_HAL::CANIface* can_iface)
308 : {
309 5 : if (!canard_iface.add_interface(can_iface)) {
310 0 : return false;
311 : }
312 5 : return true;
313 : }
314 :
315 5 : void AP_DroneCAN::init(uint8_t driver_index)
316 : {
317 5 : if (driver_index != _driver_index) {
318 0 : return;
319 : }
320 5 : if (_initialized) {
321 0 : return;
322 : }
323 5 : uint8_t node = _dronecan_node;
324 5 : if (node < 1 || node > 125) { // reset to default if invalid
325 0 : _dronecan_node.set(AP_DRONECAN_DEFAULT_NODE);
326 0 : node = AP_DRONECAN_DEFAULT_NODE;
327 : }
328 :
329 5 : 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);
330 :
331 5 : node_info_rsp.software_version.major = AP_DRONECAN_SW_VERS_MAJOR;
332 5 : node_info_rsp.software_version.minor = AP_DRONECAN_SW_VERS_MINOR;
333 5 : node_info_rsp.hardware_version.major = AP_DRONECAN_HW_VERS_MAJOR;
334 5 : node_info_rsp.hardware_version.minor = AP_DRONECAN_HW_VERS_MINOR;
335 :
336 : #if HAL_CANFD_SUPPORTED
337 5 : if (option_is_set(Options::CANFD_ENABLED)) {
338 0 : canard_iface.set_canfd(true);
339 : }
340 : #endif
341 :
342 5 : uint8_t uid_len = sizeof(uavcan_protocol_HardwareVersion::unique_id);
343 : uint8_t unique_id[sizeof(uavcan_protocol_HardwareVersion::unique_id)];
344 :
345 5 : mem_pool = NEW_NOTHROW uint32_t[_pool_size/sizeof(uint32_t)];
346 5 : if (mem_pool == nullptr) {
347 0 : return;
348 : }
349 5 : canard_iface.init(mem_pool, (_pool_size/sizeof(uint32_t))*sizeof(uint32_t), node);
350 :
351 5 : if (!hal.util->get_system_id_unformatted(unique_id, uid_len)) {
352 0 : return;
353 : }
354 5 : unique_id[uid_len - 1] += node;
355 5 : memcpy(node_info_rsp.hardware_version.unique_id, unique_id, uid_len);
356 :
357 : //Start Servers
358 5 : if (!_dna_server.init(unique_id, uid_len, node)) {
359 0 : return;
360 : }
361 :
362 : // Roundup all subscribers from supported drivers
363 5 : bool subscribed = true;
364 : #if AP_GPS_DRONECAN_ENABLED
365 5 : subscribed = subscribed && AP_GPS_DroneCAN::subscribe_msgs(this);
366 : #endif
367 : #if AP_COMPASS_DRONECAN_ENABLED
368 5 : subscribed = subscribed && AP_Compass_DroneCAN::subscribe_msgs(this);
369 : #endif
370 : #if AP_BARO_DRONECAN_ENABLED
371 5 : subscribed = subscribed && AP_Baro_DroneCAN::subscribe_msgs(this);
372 : #endif
373 5 : subscribed = subscribed && AP_BattMonitor_DroneCAN::subscribe_msgs(this);
374 : #if AP_AIRSPEED_DRONECAN_ENABLED
375 5 : subscribed = subscribed && AP_Airspeed_DroneCAN::subscribe_msgs(this);
376 : #endif
377 : #if AP_OPTICALFLOW_HEREFLOW_ENABLED
378 5 : subscribed = subscribed && AP_OpticalFlow_HereFlow::subscribe_msgs(this);
379 : #endif
380 : #if AP_RANGEFINDER_DRONECAN_ENABLED
381 5 : subscribed = subscribed && AP_RangeFinder_DroneCAN::subscribe_msgs(this);
382 : #endif
383 : #if AP_RCPROTOCOL_DRONECAN_ENABLED
384 5 : subscribed = subscribed && AP_RCProtocol_DroneCAN::subscribe_msgs(this);
385 : #endif
386 : #if AP_EFI_DRONECAN_ENABLED
387 5 : subscribed = subscribed && AP_EFI_DroneCAN::subscribe_msgs(this);
388 : #endif
389 :
390 : #if AP_PROXIMITY_DRONECAN_ENABLED
391 5 : subscribed = subscribed && AP_Proximity_DroneCAN::subscribe_msgs(this);
392 : #endif
393 : #if HAL_MOUNT_XACTI_ENABLED
394 5 : subscribed = subscribed && AP_Mount_Xacti::subscribe_msgs(this);
395 : #endif
396 : #if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED
397 5 : subscribed = subscribed && AP_TemperatureSensor_DroneCAN::subscribe_msgs(this);
398 : #endif
399 : #if AP_RPM_DRONECAN_ENABLED
400 5 : subscribed = subscribed && AP_RPM_DroneCAN::subscribe_msgs(this);
401 : #endif
402 :
403 5 : if (!subscribed) {
404 0 : AP_BoardConfig::allocation_error("DroneCAN callback");
405 : }
406 :
407 5 : act_out_array.set_timeout_ms(5);
408 5 : act_out_array.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
409 :
410 5 : esc_raw.set_timeout_ms(2);
411 : // esc_raw is one higher than high priority to ensure that it is given higher priority over act_out_array
412 5 : esc_raw.set_priority(CANARD_TRANSFER_PRIORITY_HIGH - 1);
413 :
414 : #if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
415 5 : esc_hobbywing_raw.set_timeout_ms(2);
416 5 : esc_hobbywing_raw.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
417 : #endif
418 :
419 : #if AP_DRONECAN_HIMARK_SERVO_SUPPORT
420 5 : himark_out.set_timeout_ms(2);
421 5 : himark_out.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
422 : #endif
423 :
424 5 : rgb_led.set_timeout_ms(20);
425 5 : rgb_led.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
426 :
427 5 : buzzer.set_timeout_ms(20);
428 5 : buzzer.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
429 :
430 5 : safety_state.set_timeout_ms(20);
431 5 : safety_state.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
432 :
433 5 : arming_status.set_timeout_ms(20);
434 5 : arming_status.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
435 :
436 : #if AP_DRONECAN_SEND_GPS
437 5 : gnss_fix2.set_timeout_ms(20);
438 5 : gnss_fix2.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
439 :
440 5 : gnss_auxiliary.set_timeout_ms(20);
441 5 : gnss_auxiliary.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
442 :
443 5 : gnss_heading.set_timeout_ms(20);
444 5 : gnss_heading.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
445 :
446 5 : gnss_status.set_timeout_ms(20);
447 5 : gnss_status.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
448 : #endif
449 :
450 5 : rtcm_stream.set_timeout_ms(20);
451 5 : rtcm_stream.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
452 :
453 5 : notify_state.set_timeout_ms(20);
454 5 : notify_state.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
455 :
456 : #if HAL_MOUNT_XACTI_ENABLED
457 5 : xacti_copter_att_status.set_timeout_ms(20);
458 5 : xacti_copter_att_status.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
459 5 : xacti_gimbal_control_data.set_timeout_ms(20);
460 5 : xacti_gimbal_control_data.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
461 5 : xacti_gnss_status.set_timeout_ms(20);
462 5 : xacti_gnss_status.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
463 : #endif
464 :
465 : #if AP_RELAY_DRONECAN_ENABLED
466 5 : relay_hardpoint.set_timeout_ms(20);
467 5 : relay_hardpoint.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
468 : #endif
469 :
470 5 : param_save_client.set_timeout_ms(20);
471 5 : param_save_client.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
472 :
473 5 : param_get_set_client.set_timeout_ms(20);
474 5 : param_get_set_client.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
475 :
476 5 : node_status.set_priority(CANARD_TRANSFER_PRIORITY_LOWEST);
477 5 : node_status.set_timeout_ms(1000);
478 :
479 5 : protocol_stats.set_priority(CANARD_TRANSFER_PRIORITY_LOWEST);
480 5 : protocol_stats.set_timeout_ms(3000);
481 :
482 5 : can_stats.set_priority(CANARD_TRANSFER_PRIORITY_LOWEST);
483 5 : can_stats.set_timeout_ms(3000);
484 :
485 5 : node_info_server.set_timeout_ms(20);
486 :
487 : // setup node status
488 5 : node_status_msg.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;
489 5 : node_status_msg.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
490 5 : node_status_msg.sub_mode = 0;
491 :
492 : // Spin node for device discovery
493 30 : for (uint8_t i = 0; i < 5; i++) {
494 25 : send_node_status();
495 25 : canard_iface.process(1000);
496 : }
497 :
498 5 : hal.util->snprintf(_thread_name, sizeof(_thread_name), "dronecan_%u", driver_index);
499 :
500 5 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_DroneCAN::loop, void), _thread_name, DRONECAN_STACK_SIZE, AP_HAL::Scheduler::PRIORITY_CAN, 0)) {
501 0 : return;
502 : }
503 :
504 : #if AP_DRONECAN_SERIAL_ENABLED
505 5 : serial.init(this);
506 : #endif
507 :
508 5 : _initialized = true;
509 : }
510 :
511 21802 : void AP_DroneCAN::loop(void)
512 : {
513 : while (true) {
514 21802 : if (!_initialized) {
515 3 : hal.scheduler->delay_microseconds(1000);
516 3 : continue;
517 : }
518 :
519 : // ensure that the DroneCAN thread cannot completely saturate
520 : // the CPU, preventing low priority threads from running
521 21799 : hal.scheduler->delay_microseconds(100);
522 :
523 21795 : canard_iface.process(1);
524 :
525 21794 : safety_state_send();
526 21794 : notify_state_send();
527 21794 : check_parameter_callback_timeout();
528 21794 : send_parameter_request();
529 21794 : send_parameter_save_request();
530 21794 : send_node_status();
531 21794 : _dna_server.verify_nodes();
532 :
533 : #if AP_DRONECAN_SEND_GPS && AP_GPS_DRONECAN_ENABLED
534 21794 : if (option_is_set(AP_DroneCAN::Options::SEND_GNSS) && !AP_GPS_DroneCAN::instance_exists(this)) {
535 : // send if enabled and this interface/driver is not used by the AP_GPS driver
536 0 : gnss_send_fix();
537 0 : gnss_send_yaw();
538 : }
539 : #endif
540 21794 : logging();
541 : #if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
542 21794 : hobbywing_ESC_update();
543 : #endif
544 21794 : if (_SRV_armed_mask != 0) {
545 : // we have active servos
546 0 : uint32_t now = AP_HAL::micros();
547 0 : const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get());
548 0 : if (now - _SRV_last_send_us >= servo_period_us) {
549 0 : _SRV_last_send_us = now;
550 : #if AP_DRONECAN_HIMARK_SERVO_SUPPORT
551 0 : if (option_is_set(Options::USE_HIMARK_SERVO)) {
552 0 : SRV_send_himark();
553 : } else
554 : #endif
555 : {
556 0 : SRV_send_actuator();
557 : }
558 0 : for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) {
559 0 : _SRV_conf[i].servo_pending = false;
560 : }
561 : }
562 : }
563 :
564 : #if AP_DRONECAN_SERIAL_ENABLED
565 21794 : serial.update();
566 : #endif
567 :
568 : #if AP_RELAY_DRONECAN_ENABLED
569 21794 : relay_hardpoint_send();
570 : #endif
571 21797 : }
572 : }
573 :
574 : #if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
575 21794 : void AP_DroneCAN::hobbywing_ESC_update(void)
576 : {
577 21794 : if (hal.util->get_soft_armed()) {
578 : // don't update ID database while armed, as it can cause
579 : // some hobbywing ESCs to stutter
580 14310 : return;
581 : }
582 7484 : uint32_t now = AP_HAL::millis();
583 7484 : if (now - hobbywing.last_GetId_send_ms >= 1000U) {
584 151 : hobbywing.last_GetId_send_ms = now;
585 : com_hobbywing_esc_GetEscID msg;
586 151 : msg.payload.len = 1;
587 151 : msg.payload.data[0] = 0;
588 151 : esc_hobbywing_GetEscID.broadcast(msg);
589 : }
590 : }
591 :
592 : /*
593 : handle hobbywing GetEscID reply. This gets us the mapping between CAN NodeID and throttle channel
594 : */
595 0 : void AP_DroneCAN::handle_hobbywing_GetEscID(const CanardRxTransfer& transfer, const com_hobbywing_esc_GetEscID& msg)
596 : {
597 0 : if (msg.payload.len == 2 &&
598 0 : msg.payload.data[0] == transfer.source_node_id) {
599 : // throttle channel is 2nd payload byte
600 0 : const uint8_t thr_channel = msg.payload.data[1];
601 0 : if (thr_channel > 0 && thr_channel <= HOBBYWING_MAX_ESC) {
602 0 : hobbywing.thr_chan[thr_channel-1] = transfer.source_node_id;
603 : }
604 : }
605 0 : }
606 :
607 : /*
608 : find the ESC index given a CAN node ID
609 : */
610 0 : bool AP_DroneCAN::hobbywing_find_esc_index(uint8_t node_id, uint8_t &esc_index) const
611 : {
612 0 : for (uint8_t i=0; i<HOBBYWING_MAX_ESC; i++) {
613 0 : if (hobbywing.thr_chan[i] == node_id) {
614 0 : const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
615 0 : esc_index = i + esc_offset;
616 0 : return true;
617 : }
618 : }
619 0 : return false;
620 : }
621 :
622 : /*
623 : handle hobbywing StatusMsg1 reply
624 : */
625 0 : void AP_DroneCAN::handle_hobbywing_StatusMsg1(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg1& msg)
626 : {
627 : uint8_t esc_index;
628 0 : if (hobbywing_find_esc_index(transfer.source_node_id, esc_index)) {
629 0 : update_rpm(esc_index, msg.rpm);
630 : }
631 0 : }
632 :
633 : /*
634 : handle hobbywing StatusMsg2 reply
635 : */
636 0 : void AP_DroneCAN::handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg2& msg)
637 : {
638 : uint8_t esc_index;
639 0 : if (hobbywing_find_esc_index(transfer.source_node_id, esc_index)) {
640 0 : TelemetryData t {
641 0 : .temperature_cdeg = int16_t(msg.temperature*100),
642 0 : .voltage = msg.input_voltage*0.1f,
643 0 : .current = msg.current*0.1f,
644 0 : };
645 0 : update_telem_data(esc_index, t,
646 : AP_ESC_Telem_Backend::TelemetryType::CURRENT|
647 : AP_ESC_Telem_Backend::TelemetryType::VOLTAGE|
648 : AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
649 : }
650 :
651 0 : }
652 : #endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT
653 :
654 21819 : void AP_DroneCAN::send_node_status(void)
655 : {
656 21819 : const uint32_t now = AP_HAL::millis();
657 21819 : if (now - _node_status_last_send_ms < 1000) {
658 21319 : return;
659 : }
660 500 : _node_status_last_send_ms = now;
661 500 : node_status_msg.uptime_sec = now / 1000;
662 500 : node_status.broadcast(node_status_msg);
663 :
664 500 : if (option_is_set(Options::ENABLE_STATS)) {
665 : // also send protocol and can stats
666 0 : protocol_stats.broadcast(canard_iface.protocol_stats);
667 :
668 : // get can stats
669 0 : for (uint8_t i=0; i<canard_iface.num_ifaces; i++) {
670 0 : if (canard_iface.ifaces[i] == nullptr) {
671 0 : continue;
672 : }
673 0 : auto* iface = hal.can[0];
674 0 : for (uint8_t j=0; j<HAL_NUM_CAN_IFACES; j++) {
675 0 : if (hal.can[j] == canard_iface.ifaces[i]) {
676 0 : iface = hal.can[j];
677 0 : break;
678 : }
679 : }
680 0 : auto* bus_stats = iface->get_statistics();
681 0 : if (bus_stats == nullptr) {
682 0 : continue;
683 : }
684 : dronecan_protocol_CanStats can_stats_msg;
685 0 : can_stats_msg.interface = i;
686 0 : can_stats_msg.tx_requests = bus_stats->tx_requests;
687 0 : can_stats_msg.tx_rejected = bus_stats->tx_rejected;
688 0 : can_stats_msg.tx_overflow = bus_stats->tx_overflow;
689 0 : can_stats_msg.tx_success = bus_stats->tx_success;
690 0 : can_stats_msg.tx_timedout = bus_stats->tx_timedout;
691 0 : can_stats_msg.tx_abort = bus_stats->tx_abort;
692 0 : can_stats_msg.rx_received = bus_stats->rx_received;
693 0 : can_stats_msg.rx_overflow = bus_stats->rx_overflow;
694 0 : can_stats_msg.rx_errors = bus_stats->rx_errors;
695 0 : can_stats_msg.busoff_errors = bus_stats->num_busoff_err;
696 0 : can_stats.broadcast(can_stats_msg);
697 : }
698 : }
699 : }
700 :
701 0 : void AP_DroneCAN::handle_node_info_request(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoRequest& req)
702 : {
703 0 : node_info_rsp.status = node_status_msg;
704 0 : node_info_rsp.status.uptime_sec = AP_HAL::millis() / 1000;
705 :
706 0 : node_info_server.respond(transfer, node_info_rsp);
707 0 : }
708 :
709 0 : int16_t AP_DroneCAN::scale_esc_output(uint8_t idx){
710 : static const int16_t cmd_max = ((1<<13)-1);
711 0 : float scaled = hal.rcout->scale_esc_to_unity(_SRV_conf[idx].pulse);
712 : // Prevent invalid values (from misconfigured scaling parameters) from sending non-zero commands
713 0 : if (!isfinite(scaled)) {
714 0 : return 0;
715 : }
716 0 : scaled = constrain_float(scaled, -1, 1);
717 : //Check if this channel has a reversible ESC. If it does, we can send negative commands.
718 0 : if ((((uint32_t) 1) << idx) & _esc_rv) {
719 0 : scaled *= cmd_max;
720 : } else {
721 0 : scaled = cmd_max * (scaled + 1.0) / 2.0;
722 : }
723 :
724 0 : return static_cast<int16_t>(scaled);
725 : }
726 :
727 : ///// SRV output /////
728 :
729 0 : void AP_DroneCAN::SRV_send_actuator(void)
730 : {
731 0 : uint8_t starting_servo = 0;
732 : bool repeat_send;
733 :
734 0 : WITH_SEMAPHORE(SRV_sem);
735 :
736 0 : do {
737 0 : repeat_send = false;
738 : uavcan_equipment_actuator_ArrayCommand msg;
739 :
740 : uint8_t i;
741 : // DroneCAN can hold maximum of 15 commands in one frame
742 0 : for (i = 0; starting_servo < DRONECAN_SRV_NUMBER && i < 15; starting_servo++) {
743 : uavcan_equipment_actuator_Command cmd;
744 :
745 : /*
746 : * Servo output uses a range of 1000-2000 PWM for scaling.
747 : * This converts output PWM from [1000:2000] range to [-1:1] range that
748 : * is passed to servo as unitless type via DroneCAN.
749 : * This approach allows for MIN/TRIM/MAX values to be used fully on
750 : * autopilot side and for servo it should have the setup to provide maximum
751 : * physically possible throws at [-1:1] limits.
752 : */
753 :
754 0 : if (_SRV_conf[starting_servo].servo_pending && ((((uint32_t) 1) << starting_servo) & _SRV_armed_mask)) {
755 0 : cmd.actuator_id = starting_servo + 1;
756 :
757 0 : if (option_is_set(Options::USE_ACTUATOR_PWM)) {
758 0 : cmd.command_type = UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_PWM;
759 0 : cmd.command_value = _SRV_conf[starting_servo].pulse;
760 : } else {
761 0 : cmd.command_type = UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_UNITLESS;
762 0 : cmd.command_value = constrain_float(((float) _SRV_conf[starting_servo].pulse - 1000.0) / 500.0 - 1.0, -1.0, 1.0);
763 : }
764 :
765 0 : msg.commands.data[i] = cmd;
766 :
767 0 : i++;
768 : }
769 : }
770 0 : msg.commands.len = i;
771 0 : if (i > 0) {
772 0 : if (act_out_array.broadcast(msg) > 0) {
773 0 : _srv_send_count++;
774 : } else {
775 0 : _fail_send_count++;
776 : }
777 :
778 0 : if (i == 15) {
779 0 : repeat_send = true;
780 : }
781 : }
782 : } while (repeat_send);
783 0 : }
784 :
785 : #if AP_DRONECAN_HIMARK_SERVO_SUPPORT
786 : /*
787 : Himark servo output. This uses com.himark.servo.ServoCmd packets
788 : */
789 0 : void AP_DroneCAN::SRV_send_himark(void)
790 : {
791 0 : WITH_SEMAPHORE(SRV_sem);
792 :
793 : // ServoCmd can hold maximum of 17 commands. First find the highest pending servo < 17
794 0 : int8_t highest_to_send = -1;
795 0 : for (int8_t i = 16; i >= 0; i--) {
796 0 : if (_SRV_conf[i].servo_pending && ((1U<<i) & _SRV_armed_mask) != 0) {
797 0 : highest_to_send = i;
798 0 : break;
799 : }
800 : }
801 0 : if (highest_to_send == -1) {
802 : // nothing to send
803 0 : return;
804 : }
805 0 : com_himark_servo_ServoCmd msg {};
806 :
807 0 : for (uint8_t i = 0; i <= highest_to_send; i++) {
808 0 : if ((1U<<i) & _SRV_armed_mask) {
809 0 : const uint16_t pulse = constrain_int16(_SRV_conf[i].pulse - 1000, 0, 1000);
810 0 : msg.cmd.data[i] = pulse;
811 : }
812 : }
813 0 : msg.cmd.len = highest_to_send+1;
814 :
815 0 : himark_out.broadcast(msg);
816 0 : }
817 : #endif // AP_DRONECAN_HIMARK_SERVO_SUPPORT
818 :
819 0 : void AP_DroneCAN::SRV_send_esc(void)
820 : {
821 : uavcan_equipment_esc_RawCommand esc_msg;
822 :
823 0 : uint8_t active_esc_num = 0, max_esc_num = 0;
824 0 : uint8_t k = 0;
825 :
826 : // esc offset allows for efficient packing of higher ESC numbers in RawCommand
827 0 : const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
828 :
829 : // find out how many esc we have enabled and if they are active at all
830 0 : for (uint8_t i = esc_offset; i < DRONECAN_SRV_NUMBER; i++) {
831 0 : if ((((uint32_t) 1) << i) & _ESC_armed_mask) {
832 0 : max_esc_num = i + 1;
833 0 : if (_SRV_conf[i].esc_pending) {
834 0 : active_esc_num++;
835 : }
836 : }
837 : }
838 :
839 : // if at least one is active (update) we need to send to all
840 0 : if (active_esc_num > 0) {
841 0 : k = 0;
842 0 : const bool armed = hal.util->get_soft_armed();
843 0 : for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) {
844 0 : if (armed && ((((uint32_t) 1U) << i) & _ESC_armed_mask)) {
845 0 : esc_msg.cmd.data[k] = scale_esc_output(i);
846 : } else {
847 0 : esc_msg.cmd.data[k] = static_cast<unsigned>(0);
848 : }
849 :
850 0 : k++;
851 : }
852 0 : esc_msg.cmd.len = k;
853 :
854 0 : if (esc_raw.broadcast(esc_msg)) {
855 0 : _esc_send_count++;
856 : } else {
857 0 : _fail_send_count++;
858 : }
859 : // immediately push data to CAN bus
860 0 : canard_iface.processTx(true);
861 : }
862 :
863 0 : for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) {
864 0 : _SRV_conf[i].esc_pending = false;
865 : }
866 0 : }
867 :
868 : #if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
869 : /*
870 : support for Hobbywing DroneCAN ESCs
871 : */
872 0 : void AP_DroneCAN::SRV_send_esc_hobbywing(void)
873 : {
874 : com_hobbywing_esc_RawCommand esc_msg;
875 :
876 0 : uint8_t active_esc_num = 0, max_esc_num = 0;
877 0 : uint8_t k = 0;
878 :
879 : // esc offset allows for efficient packing of higher ESC numbers in RawCommand
880 0 : const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
881 :
882 : // find out how many esc we have enabled and if they are active at all
883 0 : for (uint8_t i = esc_offset; i < DRONECAN_SRV_NUMBER; i++) {
884 0 : if ((((uint32_t) 1) << i) & _ESC_armed_mask) {
885 0 : max_esc_num = i + 1;
886 0 : if (_SRV_conf[i].esc_pending) {
887 0 : active_esc_num++;
888 : }
889 : }
890 : }
891 :
892 : // if at least one is active (update) we need to send to all
893 0 : if (active_esc_num > 0) {
894 0 : k = 0;
895 0 : const bool armed = hal.util->get_soft_armed();
896 0 : for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) {
897 0 : if (armed && ((((uint32_t) 1U) << i) & _ESC_armed_mask)) {
898 0 : esc_msg.command.data[k] = scale_esc_output(i);
899 : } else {
900 0 : esc_msg.command.data[k] = static_cast<unsigned>(0);
901 : }
902 :
903 0 : k++;
904 : }
905 0 : esc_msg.command.len = k;
906 :
907 0 : if (esc_hobbywing_raw.broadcast(esc_msg)) {
908 0 : _esc_send_count++;
909 : } else {
910 0 : _fail_send_count++;
911 : }
912 : // immediately push data to CAN bus
913 0 : canard_iface.processTx(true);
914 : }
915 0 : }
916 : #endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT
917 :
918 157616 : void AP_DroneCAN::SRV_push_servos()
919 : {
920 157616 : WITH_SEMAPHORE(SRV_sem);
921 :
922 157616 : uint32_t non_zero_channels = 0;
923 5201328 : for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) {
924 : // Check if this channels has any function assigned
925 5043712 : if (SRV_Channels::channel_function(i) >= SRV_Channel::k_none) {
926 2521856 : _SRV_conf[i].pulse = SRV_Channels::srv_channel(i)->get_output_pwm();
927 2521856 : _SRV_conf[i].esc_pending = true;
928 2521856 : _SRV_conf[i].servo_pending = true;
929 :
930 : // Flag channels which are non zero
931 2521856 : if (_SRV_conf[i].pulse > 0) {
932 1062224 : non_zero_channels |= 1U << i;
933 : }
934 : }
935 : }
936 :
937 157616 : uint32_t servo_armed_mask = _servo_bm & non_zero_channels;
938 157616 : uint32_t esc_armed_mask = _esc_bm & non_zero_channels;
939 157616 : const bool safety_off = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
940 157616 : if (!safety_off) {
941 163 : AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
942 163 : if (boardconfig != nullptr) {
943 163 : const uint32_t safety_mask = boardconfig->get_safety_mask();
944 163 : servo_armed_mask &= safety_mask;
945 163 : esc_armed_mask &= safety_mask;
946 : } else {
947 0 : servo_armed_mask = 0;
948 0 : esc_armed_mask = 0;
949 : }
950 : }
951 157616 : _SRV_armed_mask = servo_armed_mask;
952 157616 : _ESC_armed_mask = esc_armed_mask;
953 :
954 157616 : if (_ESC_armed_mask != 0) {
955 : // push ESCs as fast as we can
956 : #if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
957 0 : if (option_is_set(Options::USE_HOBBYWING_ESC)) {
958 0 : SRV_send_esc_hobbywing();
959 : } else
960 : #endif
961 : {
962 0 : SRV_send_esc();
963 : }
964 : }
965 157616 : }
966 :
967 : // notify state send
968 21794 : void AP_DroneCAN::notify_state_send()
969 : {
970 21794 : uint32_t now = AP_HAL::millis();
971 :
972 21794 : if (_notify_state_hz == 0 || (now - _last_notify_state_ms) < uint32_t(1000 / _notify_state_hz)) {
973 14291 : return;
974 : }
975 :
976 : ardupilot_indication_NotifyState msg;
977 7503 : msg.vehicle_state = 0;
978 7503 : if (AP_Notify::flags.initialising) {
979 25 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_INITIALISING;
980 : }
981 7503 : if (AP_Notify::flags.armed) {
982 5187 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_ARMED;
983 : }
984 7503 : if (AP_Notify::flags.flying) {
985 5056 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FLYING;
986 : }
987 7503 : if (AP_Notify::flags.compass_cal_running) {
988 0 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_MAGCAL_RUN;
989 : }
990 7503 : if (AP_Notify::flags.ekf_bad) {
991 0 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_EKF_BAD;
992 : }
993 7503 : if (AP_Notify::flags.esc_calibration) {
994 0 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_ESC_CALIBRATION;
995 : }
996 7503 : if (AP_Notify::flags.failsafe_battery) {
997 0 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FAILSAFE_BATT;
998 : }
999 7503 : if (AP_Notify::flags.failsafe_gcs) {
1000 0 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FAILSAFE_GCS;
1001 : }
1002 7503 : if (AP_Notify::flags.failsafe_radio) {
1003 0 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FAILSAFE_RADIO;
1004 : }
1005 7503 : if (AP_Notify::flags.firmware_update) {
1006 6 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FW_UPDATE;
1007 : }
1008 7503 : if (AP_Notify::flags.gps_fusion) {
1009 5268 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_GPS_FUSION;
1010 : }
1011 7503 : if (AP_Notify::flags.gps_glitching) {
1012 0 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_GPS_GLITCH;
1013 : }
1014 7503 : if (AP_Notify::flags.have_pos_abs) {
1015 5268 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_POS_ABS_AVAIL;
1016 : }
1017 7503 : if (AP_Notify::flags.leak_detected) {
1018 0 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_LEAK_DET;
1019 : }
1020 7503 : if (AP_Notify::flags.parachute_release) {
1021 0 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_CHUTE_RELEASED;
1022 : }
1023 7503 : if (AP_Notify::flags.powering_off) {
1024 0 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_POWERING_OFF;
1025 : }
1026 7503 : if (AP_Notify::flags.pre_arm_check) {
1027 6069 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_PREARM;
1028 : }
1029 7503 : if (AP_Notify::flags.pre_arm_gps_check) {
1030 7153 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_PREARM_GPS;
1031 : }
1032 7503 : if (AP_Notify::flags.save_trim) {
1033 0 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_SAVE_TRIM;
1034 : }
1035 7503 : if (AP_Notify::flags.vehicle_lost) {
1036 0 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_LOST;
1037 : }
1038 7503 : if (AP_Notify::flags.video_recording) {
1039 0 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_VIDEO_RECORDING;
1040 : }
1041 7503 : if (AP_Notify::flags.waiting_for_throw) {
1042 0 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_THROW_READY;
1043 : }
1044 :
1045 : #ifndef HAL_BUILD_AP_PERIPH
1046 7503 : const AP_Vehicle* vehicle = AP::vehicle();
1047 7503 : if (vehicle != nullptr) {
1048 7503 : if (vehicle->is_landing()) {
1049 1378 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_IS_LANDING;
1050 : }
1051 7503 : if (vehicle->is_taking_off()) {
1052 536 : msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_IS_TAKING_OFF;
1053 : }
1054 : }
1055 : #endif // HAL_BUILD_AP_PERIPH
1056 :
1057 : // beware that
1058 : // ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_YAW_EARTH_CENTIDEGREES
1059 : // is strange; it's number of degrees *counter-clockwise* from North.
1060 7503 : msg.aux_data_type = ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_YAW_EARTH_CENTIDEGREES;
1061 7503 : uint16_t yaw_cd = (uint16_t)(360.0f - AP::ahrs().get_yaw_deg())*100.0f;
1062 7503 : const uint8_t *data = (uint8_t *)&yaw_cd;
1063 22509 : for (uint8_t i=0; i<2; i++) {
1064 15006 : msg.aux_data.data[i] = data[i];
1065 : }
1066 7503 : msg.aux_data.len = 2;
1067 7503 : notify_state.broadcast(msg);
1068 7503 : _last_notify_state_ms = AP_HAL::millis();
1069 : }
1070 :
1071 : #if AP_DRONECAN_SEND_GPS
1072 0 : void AP_DroneCAN::gnss_send_fix()
1073 : {
1074 0 : const AP_GPS &gps = AP::gps();
1075 :
1076 0 : const uint32_t gps_lib_time_ms = gps.last_message_time_ms();
1077 0 : if (_gnss.last_gps_lib_fix_ms == gps_lib_time_ms) {
1078 0 : return;
1079 : }
1080 0 : _gnss.last_gps_lib_fix_ms = gps_lib_time_ms;
1081 :
1082 : /*
1083 : send Fix2 packet
1084 : */
1085 :
1086 0 : uavcan_equipment_gnss_Fix2 pkt {};
1087 0 : const Location &loc = gps.location();
1088 0 : const Vector3f &vel = gps.velocity();
1089 :
1090 0 : pkt.timestamp.usec = AP_HAL::micros64();
1091 0 : pkt.gnss_timestamp.usec = gps.time_epoch_usec();
1092 0 : if (pkt.gnss_timestamp.usec == 0) {
1093 0 : pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_NONE;
1094 : } else {
1095 0 : pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_UTC;
1096 : }
1097 0 : pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL;
1098 0 : pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL;
1099 0 : pkt.height_ellipsoid_mm = loc.alt * 10;
1100 0 : pkt.height_msl_mm = loc.alt * 10;
1101 0 : for (uint8_t i=0; i<3; i++) {
1102 0 : pkt.ned_velocity[i] = vel[i];
1103 : }
1104 0 : pkt.sats_used = gps.num_sats();
1105 0 : switch (gps.status()) {
1106 0 : case AP_GPS_FixType::NO_GPS:
1107 : case AP_GPS_FixType::NONE:
1108 0 : pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_NO_FIX;
1109 0 : pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE;
1110 0 : pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER;
1111 0 : break;
1112 0 : case AP_GPS_FixType::FIX_2D:
1113 0 : pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_2D_FIX;
1114 0 : pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE;
1115 0 : pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER;
1116 0 : break;
1117 0 : case AP_GPS_FixType::FIX_3D:
1118 0 : pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
1119 0 : pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE;
1120 0 : pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER;
1121 0 : break;
1122 0 : case AP_GPS_FixType::DGPS:
1123 0 : pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
1124 0 : pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_DGPS;
1125 0 : pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_SBAS;
1126 0 : break;
1127 0 : case AP_GPS_FixType::RTK_FLOAT:
1128 0 : pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
1129 0 : pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK;
1130 0 : pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FLOAT;
1131 0 : break;
1132 0 : case AP_GPS_FixType::RTK_FIXED:
1133 0 : pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
1134 0 : pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK;
1135 0 : pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FIXED;
1136 0 : break;
1137 0 : case AP_GPS_FixType::STATIC:
1138 : case AP_GPS_FixType::PPP:
1139 0 : pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
1140 0 : pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_PPP; // Static is not representable in DroneCAN.
1141 0 : pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FIXED; // There is no submode for static or PPP
1142 0 : break;
1143 : }
1144 :
1145 0 : pkt.covariance.len = 6;
1146 : float hacc;
1147 0 : if (gps.horizontal_accuracy(hacc)) {
1148 0 : pkt.covariance.data[0] = pkt.covariance.data[1] = sq(hacc);
1149 : }
1150 : float vacc;
1151 0 : if (gps.vertical_accuracy(vacc)) {
1152 0 : pkt.covariance.data[2] = sq(vacc);
1153 : }
1154 : float sacc;
1155 0 : if (gps.speed_accuracy(sacc)) {
1156 0 : const float vc3 = sq(sacc);
1157 0 : pkt.covariance.data[3] = pkt.covariance.data[4] = pkt.covariance.data[5] = vc3;
1158 : }
1159 :
1160 0 : gnss_fix2.broadcast(pkt);
1161 :
1162 :
1163 :
1164 0 : const uint32_t now_ms = AP_HAL::millis();
1165 0 : if (now_ms - _gnss.last_send_status_ms >= 1000) {
1166 0 : _gnss.last_send_status_ms = now_ms;
1167 :
1168 : /*
1169 : send aux packet
1170 : */
1171 0 : uavcan_equipment_gnss_Auxiliary pkt_auxiliary {};
1172 0 : pkt_auxiliary.hdop = gps.get_hdop() * 0.01;
1173 0 : pkt_auxiliary.vdop = gps.get_vdop() * 0.01;
1174 :
1175 0 : gnss_auxiliary.broadcast(pkt_auxiliary);
1176 :
1177 :
1178 : /*
1179 : send Status packet
1180 : */
1181 0 : ardupilot_gnss_Status pkt_status {};
1182 0 : pkt_status.healthy = gps.is_healthy();
1183 0 : if (gps.logging_present() && gps.logging_enabled() && !gps.logging_failed()) {
1184 0 : pkt_status.status |= ARDUPILOT_GNSS_STATUS_STATUS_LOGGING;
1185 : }
1186 : uint8_t idx; // unused
1187 0 : if (pkt_status.healthy && !gps.first_unconfigured_gps(idx)) {
1188 0 : pkt_status.status |= ARDUPILOT_GNSS_STATUS_STATUS_ARMABLE;
1189 : }
1190 :
1191 : uint32_t error_codes;
1192 0 : if (gps.get_error_codes(error_codes)) {
1193 0 : pkt_status.error_codes = error_codes;
1194 : }
1195 :
1196 0 : gnss_status.broadcast(pkt_status);
1197 : }
1198 : }
1199 :
1200 0 : void AP_DroneCAN::gnss_send_yaw()
1201 : {
1202 0 : const AP_GPS &gps = AP::gps();
1203 :
1204 0 : if (!gps.have_gps_yaw()) {
1205 0 : return;
1206 : }
1207 :
1208 : float yaw_deg, yaw_acc_deg;
1209 : uint32_t yaw_time_ms;
1210 0 : if (!gps.gps_yaw_deg(yaw_deg, yaw_acc_deg, yaw_time_ms) && yaw_time_ms != _gnss.last_lib_yaw_time_ms) {
1211 0 : return;
1212 : }
1213 :
1214 0 : _gnss.last_lib_yaw_time_ms = yaw_time_ms;
1215 :
1216 0 : ardupilot_gnss_Heading pkt_heading {};
1217 0 : pkt_heading.heading_valid = true;
1218 0 : pkt_heading.heading_accuracy_valid = is_positive(yaw_acc_deg);
1219 0 : pkt_heading.heading_rad = radians(yaw_deg);
1220 0 : pkt_heading.heading_accuracy_rad = radians(yaw_acc_deg);
1221 :
1222 0 : gnss_heading.broadcast(pkt_heading);
1223 : }
1224 : #endif // AP_DRONECAN_SEND_GPS
1225 :
1226 : // SafetyState send
1227 21794 : void AP_DroneCAN::safety_state_send()
1228 : {
1229 21794 : uint32_t now = AP_HAL::millis();
1230 21794 : if (now - _last_safety_state_ms < 500) {
1231 : // update at 2Hz
1232 20849 : return;
1233 : }
1234 945 : _last_safety_state_ms = now;
1235 :
1236 : { // handle SafetyState
1237 : ardupilot_indication_SafetyState safety_msg;
1238 945 : auto state = hal.util->safety_switch_state();
1239 945 : if (_SRV_armed_mask != 0 || _ESC_armed_mask != 0) {
1240 : // if we are outputting any servos or ESCs due to
1241 : // BRD_SAFETY_MASK then we need to advertise safety as
1242 : // off, this changes LEDs to indicate unsafe and allows
1243 : // AP_Periph ESCs and servos to run
1244 0 : state = AP_HAL::Util::SAFETY_ARMED;
1245 : }
1246 945 : switch (state) {
1247 921 : case AP_HAL::Util::SAFETY_ARMED:
1248 921 : safety_msg.status = ARDUPILOT_INDICATION_SAFETYSTATE_STATUS_SAFETY_OFF;
1249 921 : safety_state.broadcast(safety_msg);
1250 921 : break;
1251 24 : case AP_HAL::Util::SAFETY_DISARMED:
1252 24 : safety_msg.status = ARDUPILOT_INDICATION_SAFETYSTATE_STATUS_SAFETY_ON;
1253 24 : safety_state.broadcast(safety_msg);
1254 24 : break;
1255 0 : default:
1256 : // nothing to send
1257 0 : break;
1258 : }
1259 : }
1260 :
1261 : { // handle ArmingStatus
1262 : uavcan_equipment_safety_ArmingStatus arming_msg;
1263 945 : arming_msg.status = hal.util->get_soft_armed() ? UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_FULLY_ARMED :
1264 : UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_DISARMED;
1265 945 : arming_status.broadcast(arming_msg);
1266 : }
1267 : }
1268 :
1269 : // Send relay outputs with hardpoint msg
1270 : #if AP_RELAY_DRONECAN_ENABLED
1271 21794 : void AP_DroneCAN::relay_hardpoint_send()
1272 : {
1273 21794 : const uint32_t now = AP_HAL::millis();
1274 21794 : if ((_relay.rate_hz == 0) || ((now - _relay.last_send_ms) < uint32_t(1000 / _relay.rate_hz))) {
1275 : // Rate limit per user config
1276 21794 : return;
1277 : }
1278 0 : _relay.last_send_ms = now;
1279 :
1280 0 : AP_Relay *relay = AP::relay();
1281 0 : if (relay == nullptr) {
1282 0 : return;
1283 : }
1284 :
1285 0 : uavcan_equipment_hardpoint_Command msg {};
1286 :
1287 : // Relay will populate the next command to send and update the last index
1288 : // This will cycle through each relay in turn
1289 0 : if (relay->dronecan.populate_next_command(_relay.last_index, msg)) {
1290 0 : relay_hardpoint.broadcast(msg);
1291 : }
1292 :
1293 : }
1294 : #endif // AP_RELAY_DRONECAN_ENABLED
1295 :
1296 : /*
1297 : handle Button message
1298 : */
1299 0 : void AP_DroneCAN::handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg)
1300 : {
1301 0 : switch (msg.button) {
1302 0 : case ARDUPILOT_INDICATION_BUTTON_BUTTON_SAFETY: {
1303 0 : AP_BoardConfig *brdconfig = AP_BoardConfig::get_singleton();
1304 0 : if (brdconfig && brdconfig->safety_button_handle_pressed(msg.press_time)) {
1305 0 : AP_HAL::Util::safety_state state = hal.util->safety_switch_state();
1306 0 : if (state == AP_HAL::Util::SAFETY_ARMED) {
1307 0 : hal.rcout->force_safety_on();
1308 : } else {
1309 0 : hal.rcout->force_safety_off();
1310 : }
1311 : }
1312 0 : break;
1313 : }
1314 : }
1315 0 : }
1316 :
1317 : /*
1318 : handle traffic report
1319 : */
1320 0 : void AP_DroneCAN::handle_traffic_report(const CanardRxTransfer& transfer, const ardupilot_equipment_trafficmonitor_TrafficReport& msg)
1321 : {
1322 : #if HAL_ADSB_ENABLED
1323 0 : AP_ADSB *adsb = AP::ADSB();
1324 0 : if (!adsb || !adsb->enabled()) {
1325 : // ADSB not enabled
1326 0 : return;
1327 : }
1328 :
1329 : AP_ADSB::adsb_vehicle_t vehicle;
1330 0 : mavlink_adsb_vehicle_t &pkt = vehicle.info;
1331 :
1332 0 : pkt.ICAO_address = msg.icao_address;
1333 0 : pkt.tslc = msg.tslc;
1334 0 : pkt.lat = msg.latitude_deg_1e7;
1335 0 : pkt.lon = msg.longitude_deg_1e7;
1336 0 : pkt.altitude = msg.alt_m * 1000;
1337 0 : pkt.heading = degrees(msg.heading) * 100;
1338 0 : pkt.hor_velocity = norm(msg.velocity[0], msg.velocity[1]) * 100;
1339 0 : pkt.ver_velocity = -msg.velocity[2] * 100;
1340 0 : pkt.squawk = msg.squawk;
1341 0 : for (uint8_t i=0; i<9; i++) {
1342 0 : pkt.callsign[i] = msg.callsign[i];
1343 : }
1344 0 : pkt.emitter_type = msg.traffic_type;
1345 :
1346 0 : if (msg.alt_type == ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_PRESSURE_AMSL) {
1347 0 : pkt.flags |= ADSB_FLAGS_VALID_ALTITUDE;
1348 0 : pkt.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH;
1349 0 : } else if (msg.alt_type == ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_WGS84) {
1350 0 : pkt.flags |= ADSB_FLAGS_VALID_ALTITUDE;
1351 0 : pkt.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC;
1352 : }
1353 :
1354 0 : if (msg.lat_lon_valid) {
1355 0 : pkt.flags |= ADSB_FLAGS_VALID_COORDS;
1356 : }
1357 0 : if (msg.heading_valid) {
1358 0 : pkt.flags |= ADSB_FLAGS_VALID_HEADING;
1359 : }
1360 0 : if (msg.velocity_valid) {
1361 0 : pkt.flags |= ADSB_FLAGS_VALID_VELOCITY;
1362 : }
1363 0 : if (msg.callsign_valid) {
1364 0 : pkt.flags |= ADSB_FLAGS_VALID_CALLSIGN;
1365 : }
1366 0 : if (msg.ident_valid) {
1367 0 : pkt.flags |= ADSB_FLAGS_VALID_SQUAWK;
1368 : }
1369 0 : if (msg.simulated_report) {
1370 0 : pkt.flags |= ADSB_FLAGS_SIMULATED;
1371 : }
1372 0 : if (msg.vertical_velocity_valid) {
1373 0 : pkt.flags |= ADSB_FLAGS_VERTICAL_VELOCITY_VALID;
1374 : }
1375 0 : if (msg.baro_valid) {
1376 0 : pkt.flags |= ADSB_FLAGS_BARO_VALID;
1377 : }
1378 :
1379 0 : vehicle.last_update_ms = AP_HAL::millis() - (vehicle.info.tslc * 1000);
1380 0 : adsb->handle_adsb_vehicle(vehicle);
1381 : #endif
1382 : }
1383 :
1384 : /*
1385 : handle actuator status message
1386 : */
1387 : #if AP_SERVO_TELEM_ENABLED
1388 0 : void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg)
1389 : {
1390 0 : AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
1391 0 : if (servo_telem == nullptr) {
1392 0 : return;
1393 : }
1394 :
1395 0 : const AP_Servo_Telem::TelemetryData telem_data {
1396 0 : .measured_position = degrees(msg.position),
1397 0 : .force = msg.force,
1398 0 : .speed = msg.speed,
1399 0 : .duty_cycle = msg.power_rating_pct,
1400 : .present_types = AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
1401 : AP_Servo_Telem::TelemetryData::Types::FORCE |
1402 : AP_Servo_Telem::TelemetryData::Types::SPEED |
1403 : AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE
1404 0 : };
1405 :
1406 0 : servo_telem->update_telem_data(msg.actuator_id - 1, telem_data);
1407 : }
1408 : #endif
1409 :
1410 : #if AP_SERVO_TELEM_ENABLED || AP_DRONECAN_LOG_CIRCUIT_STATUS_ENABLED
1411 : /*
1412 : handle circuit status message
1413 : */
1414 0 : void AP_DroneCAN::handle_circuit_status(const CanardRxTransfer& transfer, const uavcan_equipment_power_CircuitStatus& msg)
1415 : {
1416 : #if AP_SERVO_TELEM_ENABLED
1417 0 : if (msg.circuit_id >= 1 && msg.circuit_id <= DRONECAN_SRV_NUMBER &&
1418 0 : (_servo_bm & (1U << (msg.circuit_id - 1)))) {
1419 : // circuit_id maps to a channel we are driving as a DroneCAN servo,
1420 : // route into AP_Servo_Telem so it is logged alongside other servo telemetry
1421 0 : const uint8_t servo_index = msg.circuit_id - 1;
1422 :
1423 0 : AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
1424 0 : if (servo_telem != nullptr && servo_telem->is_active(servo_index)) {
1425 0 : const AP_Servo_Telem::TelemetryData telem_data {
1426 0 : .voltage = msg.voltage,
1427 0 : .current = msg.current,
1428 0 : .status_flags = msg.error_flags,
1429 : .present_types = AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
1430 : AP_Servo_Telem::TelemetryData::Types::CURRENT |
1431 : AP_Servo_Telem::TelemetryData::Types::STATUS
1432 0 : };
1433 0 : servo_telem->update_telem_data(servo_index, telem_data);
1434 : }
1435 0 : return;
1436 : }
1437 : #endif // AP_SERVO_TELEM_ENABLED
1438 :
1439 : #if AP_DRONECAN_LOG_CIRCUIT_STATUS_ENABLED && HAL_LOGGING_ENABLED
1440 : if (AP::logger().logging_enabled()) {
1441 : // @LoggerMessage: CSCU
1442 : // @Description: Generic DroneCAN circuit status
1443 : // @Field: TimeUS: Time since system startup
1444 : // @Field: I: driver index
1445 : // @Field: Id: circuit id
1446 : // @Field: V: Voltage
1447 : // @Field: A: Current
1448 : // @Field: Err: error flags
1449 : AP::logger().WriteStreaming("CSCU",
1450 : "TimeUS,I,Id,V,A,Err",
1451 : "s#-vA-",
1452 : "F-----",
1453 : "QBHffB",
1454 : AP_HAL::micros64(),
1455 : _driver_index,
1456 : msg.circuit_id,
1457 : msg.voltage,
1458 : msg.current,
1459 : msg.error_flags);
1460 : }
1461 : #endif // AP_DRONECAN_LOG_CIRCUIT_STATUS_ENABLED && HAL_LOGGING_ENABLED
1462 : }
1463 : #endif // AP_SERVO_TELEM_ENABLED || AP_DRONECAN_LOG_CIRCUIT_STATUS_ENABLED
1464 :
1465 : #if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED
1466 : /*
1467 : handle himark ServoInfo message
1468 : */
1469 0 : void AP_DroneCAN::handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg)
1470 : {
1471 0 : AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
1472 0 : if (servo_telem == nullptr) {
1473 0 : return;
1474 : }
1475 :
1476 0 : const AP_Servo_Telem::TelemetryData telem_data {
1477 0 : .command_position = msg.pos_cmd * 0.01,
1478 0 : .measured_position = msg.pos_sensor * 0.01,
1479 0 : .voltage = msg.voltage * 0.01,
1480 0 : .current = msg.current * 0.01,
1481 0 : .motor_temperature_cdeg = int16_t(((msg.motor_temp * 0.2) - 40) * 100),
1482 0 : .pcb_temperature_cdeg = int16_t(((msg.pcb_temp * 0.2) - 40) * 100),
1483 0 : .status_flags = msg.error_status,
1484 : .present_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION |
1485 : AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
1486 : AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
1487 : AP_Servo_Telem::TelemetryData::Types::CURRENT |
1488 : AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP |
1489 : AP_Servo_Telem::TelemetryData::Types::PCB_TEMP |
1490 : AP_Servo_Telem::TelemetryData::Types::STATUS
1491 0 : };
1492 :
1493 0 : servo_telem->update_telem_data(msg.servo_id - 1, telem_data);
1494 : }
1495 : #endif // AP_DRONECAN_HIMARK_SERVO_SUPPORT
1496 :
1497 : #if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
1498 : void AP_DroneCAN::handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg)
1499 : {
1500 : AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
1501 : if (servo_telem == nullptr) {
1502 : return;
1503 : }
1504 :
1505 : const AP_Servo_Telem::TelemetryData telem_data {
1506 : .measured_position = degrees(msg.actual_position),
1507 : .voltage = msg.voltage * 0.2,
1508 : .current = msg.current * 0.025,
1509 : .duty_cycle = uint8_t(msg.motor_pwm * (100.0/255.0)),
1510 : .motor_temperature_cdeg = int16_t((msg.motor_temperature - 50) * 100),
1511 : .present_types = AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
1512 : AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
1513 : AP_Servo_Telem::TelemetryData::Types::CURRENT |
1514 : AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE |
1515 : AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP
1516 : };
1517 :
1518 : servo_telem->update_telem_data(msg.actuator_id - 1, telem_data);
1519 : }
1520 : #endif
1521 :
1522 : /*
1523 : handle ESC status message
1524 : */
1525 0 : void AP_DroneCAN::handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg)
1526 : {
1527 : #if HAL_WITH_ESC_TELEM
1528 0 : const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
1529 0 : const uint8_t esc_index = msg.esc_index + esc_offset;
1530 :
1531 0 : if (!is_esc_data_index_valid(esc_index)) {
1532 0 : return;
1533 : }
1534 :
1535 0 : TelemetryData t {
1536 0 : .temperature_cdeg = int16_t((KELVIN_TO_C(msg.temperature)) * 100),
1537 0 : .voltage = msg.voltage,
1538 0 : .current = msg.current,
1539 : #if AP_EXTENDED_ESC_TELEM_ENABLED
1540 0 : .power_percentage = msg.power_rating_pct,
1541 : #endif
1542 0 : };
1543 :
1544 0 : update_rpm(esc_index, msg.rpm, msg.error_count);
1545 0 : update_telem_data(esc_index, t,
1546 0 : (isnan(msg.current) ? 0 : AP_ESC_Telem_Backend::TelemetryType::CURRENT)
1547 0 : | (isnan(msg.voltage) ? 0 : AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)
1548 0 : | (isnan(msg.temperature) ? 0 : AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE)
1549 : #if AP_EXTENDED_ESC_TELEM_ENABLED
1550 0 : | AP_ESC_Telem_Backend::TelemetryType::POWER_PERCENTAGE
1551 : #endif
1552 : );
1553 : #endif // HAL_WITH_ESC_TELEM
1554 : }
1555 :
1556 : #if AP_EXTENDED_ESC_TELEM_ENABLED
1557 : /*
1558 : handle Extended ESC status message
1559 : */
1560 0 : void AP_DroneCAN::handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg)
1561 : {
1562 0 : const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
1563 0 : const uint8_t esc_index = msg.esc_index + esc_offset;
1564 :
1565 0 : if (!is_esc_data_index_valid(esc_index)) {
1566 0 : return;
1567 : }
1568 :
1569 0 : TelemetryData telemetryData {
1570 0 : .motor_temp_cdeg = (int16_t)(msg.motor_temperature_degC * 100),
1571 0 : .input_duty = msg.input_pct,
1572 0 : .output_duty = msg.output_pct,
1573 0 : .flags = msg.status_flags,
1574 0 : };
1575 :
1576 0 : update_telem_data(esc_index, telemetryData,
1577 : AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE
1578 : | AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY
1579 : | AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY
1580 : | AP_ESC_Telem_Backend::TelemetryType::FLAGS);
1581 : }
1582 : #endif // AP_EXTENDED_ESC_TELEM_ENABLED
1583 :
1584 0 : bool AP_DroneCAN::is_esc_data_index_valid(const uint8_t index) {
1585 0 : if (index > DRONECAN_SRV_NUMBER) {
1586 : // printf("DroneCAN: invalid esc index: %d. max index allowed: %d\n\r", index, DRONECAN_SRV_NUMBER);
1587 0 : return false;
1588 : }
1589 0 : return true;
1590 : }
1591 :
1592 : #if AP_SCRIPTING_ENABLED
1593 : /*
1594 : handle FlexDebug message, holding a copy locally for a lua script to access
1595 : */
1596 0 : void AP_DroneCAN::handle_FlexDebug(const CanardRxTransfer& transfer, const dronecan_protocol_FlexDebug &msg)
1597 : {
1598 0 : if (!option_is_set(Options::ENABLE_FLEX_DEBUG)) {
1599 0 : return;
1600 : }
1601 :
1602 : // find an existing element in the list
1603 0 : const uint8_t source_node = transfer.source_node_id;
1604 0 : for (auto *p = flexDebug_list; p != nullptr; p = p->next) {
1605 0 : if (p->node_id == source_node && p->msg.id == msg.id) {
1606 0 : p->msg = msg;
1607 0 : p->timestamp_us = uint32_t(transfer.timestamp_usec);
1608 0 : return;
1609 : }
1610 : }
1611 :
1612 : // new message ID, add to the list. Note that this gets called
1613 : // only from one thread, so no lock needed
1614 0 : auto *p = NEW_NOTHROW FlexDebug;
1615 0 : if (p == nullptr) {
1616 0 : return;
1617 : }
1618 0 : p->node_id = source_node;
1619 0 : p->msg = msg;
1620 0 : p->timestamp_us = uint32_t(transfer.timestamp_usec);
1621 0 : p->next = flexDebug_list;
1622 :
1623 : // link into the list
1624 0 : flexDebug_list = p;
1625 : }
1626 :
1627 : /*
1628 : get the last FlexDebug message from a node
1629 : */
1630 0 : bool AP_DroneCAN::get_FlexDebug(uint8_t node_id, uint16_t msg_id, uint32_t ×tamp_us, dronecan_protocol_FlexDebug &msg) const
1631 : {
1632 0 : for (const auto *p = flexDebug_list; p != nullptr; p = p->next) {
1633 0 : if (p->node_id == node_id && p->msg.id == msg_id) {
1634 0 : if (timestamp_us == p->timestamp_us) {
1635 : // stale message
1636 0 : return false;
1637 : }
1638 0 : timestamp_us = p->timestamp_us;
1639 0 : msg = p->msg;
1640 0 : return true;
1641 : }
1642 : }
1643 0 : return false;
1644 : }
1645 :
1646 : #endif // AP_SCRIPTING_ENABLED
1647 :
1648 : /*
1649 : handle LogMessage debug
1650 : */
1651 0 : void AP_DroneCAN::handle_debug(const CanardRxTransfer& transfer, const uavcan_protocol_debug_LogMessage& msg)
1652 : {
1653 : #if AP_HAVE_GCS_SEND_TEXT
1654 0 : const auto log_level = AP::can().get_log_level();
1655 0 : const auto msg_level = msg.level.value;
1656 0 : bool send_mavlink = false;
1657 :
1658 0 : if (log_level != AP_CANManager::LOG_NONE) {
1659 : // log to onboard log and mavlink
1660 0 : enum MAV_SEVERITY mavlink_level = MAV_SEVERITY_INFO;
1661 0 : switch (msg_level) {
1662 0 : case UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_DEBUG:
1663 0 : mavlink_level = MAV_SEVERITY_DEBUG;
1664 0 : send_mavlink = uint8_t(log_level) >= uint8_t(AP_CANManager::LogLevel::LOG_DEBUG);
1665 0 : break;
1666 0 : case UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_INFO:
1667 0 : mavlink_level = MAV_SEVERITY_INFO;
1668 0 : send_mavlink = uint8_t(log_level) >= uint8_t(AP_CANManager::LogLevel::LOG_INFO);
1669 0 : break;
1670 0 : case UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_WARNING:
1671 0 : mavlink_level = MAV_SEVERITY_WARNING;
1672 0 : send_mavlink = uint8_t(log_level) >= uint8_t(AP_CANManager::LogLevel::LOG_WARNING);
1673 0 : break;
1674 0 : default:
1675 0 : send_mavlink = uint8_t(log_level) >= uint8_t(AP_CANManager::LogLevel::LOG_ERROR);
1676 0 : mavlink_level = MAV_SEVERITY_ERROR;
1677 0 : break;
1678 : }
1679 0 : if (send_mavlink) {
1680 : // when we send as MAVLink it also gets logged locally, so
1681 : // we return to avoid a duplicate
1682 0 : GCS_SEND_TEXT(mavlink_level, "CAN[%u] %s", transfer.source_node_id, msg.text.data);
1683 0 : return;
1684 : }
1685 : }
1686 : #endif // AP_HAVE_GCS_SEND_TEXT
1687 :
1688 : #if HAL_LOGGING_ENABLED
1689 : // always log locally if we have logging enabled
1690 0 : AP::logger().Write_MessageF("CAN[%u] %s", transfer.source_node_id, msg.text.data);
1691 : #endif
1692 : }
1693 :
1694 : /*
1695 : check for parameter get/set response timeout
1696 : */
1697 21794 : void AP_DroneCAN::check_parameter_callback_timeout()
1698 : {
1699 21794 : WITH_SEMAPHORE(_param_sem);
1700 :
1701 : // return immediately if not waiting for get/set parameter response
1702 21794 : if (param_request_sent_ms == 0) {
1703 21794 : return;
1704 : }
1705 :
1706 0 : const uint32_t now_ms = AP_HAL::millis();
1707 0 : if (now_ms - param_request_sent_ms > AP_DRONECAN_GETSET_TIMEOUT_MS) {
1708 0 : param_request_sent_ms = 0;
1709 0 : param_int_cb = nullptr;
1710 0 : param_float_cb = nullptr;
1711 0 : param_string_cb = nullptr;
1712 : }
1713 21794 : }
1714 :
1715 : /*
1716 : send any queued request to get/set parameter
1717 : called from loop
1718 : */
1719 21794 : void AP_DroneCAN::send_parameter_request()
1720 : {
1721 21794 : WITH_SEMAPHORE(_param_sem);
1722 21794 : if (param_request_sent) {
1723 21794 : return;
1724 : }
1725 0 : param_get_set_client.request(param_request_node_id, param_getset_req);
1726 0 : param_request_sent = true;
1727 21794 : }
1728 :
1729 : /*
1730 : set named float parameter on node
1731 : */
1732 0 : bool AP_DroneCAN::set_parameter_on_node(uint8_t node_id, const char *name, float value, ParamGetSetFloatCb *cb)
1733 : {
1734 0 : WITH_SEMAPHORE(_param_sem);
1735 :
1736 : // fail if waiting for any previous get/set request
1737 0 : if (param_int_cb != nullptr ||
1738 0 : param_float_cb != nullptr ||
1739 0 : param_string_cb != nullptr) {
1740 0 : return false;
1741 : }
1742 0 : param_getset_req.index = 0;
1743 0 : param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data)-1);
1744 0 : param_getset_req.value.real_value = value;
1745 0 : param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE;
1746 0 : param_float_cb = cb;
1747 0 : param_request_sent = false;
1748 0 : param_request_sent_ms = AP_HAL::millis();
1749 0 : param_request_node_id = node_id;
1750 0 : return true;
1751 0 : }
1752 :
1753 : /*
1754 : set named integer parameter on node
1755 : */
1756 0 : bool AP_DroneCAN::set_parameter_on_node(uint8_t node_id, const char *name, int32_t value, ParamGetSetIntCb *cb)
1757 : {
1758 0 : WITH_SEMAPHORE(_param_sem);
1759 :
1760 : // fail if waiting for any previous get/set request
1761 0 : if (param_int_cb != nullptr ||
1762 0 : param_float_cb != nullptr ||
1763 0 : param_string_cb != nullptr) {
1764 0 : return false;
1765 : }
1766 0 : param_getset_req.index = 0;
1767 0 : param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data)-1);
1768 0 : param_getset_req.value.integer_value = value;
1769 0 : param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
1770 0 : param_int_cb = cb;
1771 0 : param_request_sent = false;
1772 0 : param_request_sent_ms = AP_HAL::millis();
1773 0 : param_request_node_id = node_id;
1774 0 : return true;
1775 0 : }
1776 :
1777 : /*
1778 : set named string parameter on node
1779 : */
1780 0 : bool AP_DroneCAN::set_parameter_on_node(uint8_t node_id, const char *name, const string &value, ParamGetSetStringCb *cb)
1781 : {
1782 0 : WITH_SEMAPHORE(_param_sem);
1783 :
1784 : // fail if waiting for any previous get/set request
1785 0 : if (param_int_cb != nullptr ||
1786 0 : param_float_cb != nullptr ||
1787 0 : param_string_cb != nullptr) {
1788 0 : return false;
1789 : }
1790 0 : param_getset_req.index = 0;
1791 0 : param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data)-1);
1792 0 : memcpy(¶m_getset_req.value.string_value, (const void*)&value, sizeof(value));
1793 0 : param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE;
1794 0 : param_string_cb = cb;
1795 0 : param_request_sent = false;
1796 0 : param_request_sent_ms = AP_HAL::millis();
1797 0 : param_request_node_id = node_id;
1798 0 : return true;
1799 0 : }
1800 :
1801 : /*
1802 : get named float parameter on node
1803 : */
1804 0 : bool AP_DroneCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetFloatCb *cb)
1805 : {
1806 0 : WITH_SEMAPHORE(_param_sem);
1807 :
1808 : // fail if waiting for any previous get/set request
1809 0 : if (param_int_cb != nullptr ||
1810 0 : param_float_cb != nullptr ||
1811 0 : param_string_cb != nullptr) {
1812 0 : return false;
1813 : }
1814 0 : param_getset_req.index = 0;
1815 0 : param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data));
1816 0 : param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY;
1817 0 : param_float_cb = cb;
1818 0 : param_request_sent = false;
1819 0 : param_request_sent_ms = AP_HAL::millis();
1820 0 : param_request_node_id = node_id;
1821 0 : return true;
1822 0 : }
1823 :
1824 : /*
1825 : get named integer parameter on node
1826 : */
1827 0 : bool AP_DroneCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetIntCb *cb)
1828 : {
1829 0 : WITH_SEMAPHORE(_param_sem);
1830 :
1831 : // fail if waiting for any previous get/set request
1832 0 : if (param_int_cb != nullptr ||
1833 0 : param_float_cb != nullptr ||
1834 0 : param_string_cb != nullptr) {
1835 0 : return false;
1836 : }
1837 0 : param_getset_req.index = 0;
1838 0 : param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data));
1839 0 : param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY;
1840 0 : param_int_cb = cb;
1841 0 : param_request_sent = false;
1842 0 : param_request_sent_ms = AP_HAL::millis();
1843 0 : param_request_node_id = node_id;
1844 0 : return true;
1845 0 : }
1846 :
1847 : /*
1848 : get named string parameter on node
1849 : */
1850 0 : bool AP_DroneCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetStringCb *cb)
1851 : {
1852 0 : WITH_SEMAPHORE(_param_sem);
1853 :
1854 : // fail if waiting for any previous get/set request
1855 0 : if (param_int_cb != nullptr ||
1856 0 : param_float_cb != nullptr ||
1857 0 : param_string_cb != nullptr) {
1858 0 : return false;
1859 : }
1860 0 : param_getset_req.index = 0;
1861 0 : param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data));
1862 0 : param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY;
1863 0 : param_string_cb = cb;
1864 0 : param_request_sent = false;
1865 0 : param_request_sent_ms = AP_HAL::millis();
1866 0 : param_request_node_id = node_id;
1867 0 : return true;
1868 0 : }
1869 :
1870 0 : void AP_DroneCAN::handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp)
1871 : {
1872 0 : WITH_SEMAPHORE(_param_sem);
1873 0 : if (!param_int_cb &&
1874 0 : !param_float_cb &&
1875 0 : !param_string_cb) {
1876 0 : return;
1877 : }
1878 0 : if ((rsp.value.union_tag == UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE) && param_int_cb) {
1879 0 : int32_t val = rsp.value.integer_value;
1880 0 : if ((*param_int_cb)(this, transfer.source_node_id, (const char*)rsp.name.data, val)) {
1881 : // we want the parameter to be set with val
1882 0 : param_getset_req.index = 0;
1883 0 : memcpy(param_getset_req.name.data, rsp.name.data, rsp.name.len);
1884 0 : param_getset_req.value.integer_value = val;
1885 0 : param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
1886 0 : param_request_sent = false;
1887 0 : param_request_sent_ms = AP_HAL::millis();
1888 0 : param_request_node_id = transfer.source_node_id;
1889 0 : return;
1890 : }
1891 0 : } else if ((rsp.value.union_tag == UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE) && param_float_cb) {
1892 0 : float val = rsp.value.real_value;
1893 0 : if ((*param_float_cb)(this, transfer.source_node_id, (const char*)rsp.name.data, val)) {
1894 : // we want the parameter to be set with val
1895 0 : param_getset_req.index = 0;
1896 0 : memcpy(param_getset_req.name.data, rsp.name.data, rsp.name.len);
1897 0 : param_getset_req.value.real_value = val;
1898 0 : param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE;
1899 0 : param_request_sent = false;
1900 0 : param_request_sent_ms = AP_HAL::millis();
1901 0 : param_request_node_id = transfer.source_node_id;
1902 0 : return;
1903 : }
1904 0 : } else if ((rsp.value.union_tag == UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE) && param_string_cb) {
1905 : string val;
1906 0 : memcpy(&val, &rsp.value.string_value, sizeof(val));
1907 0 : if ((*param_string_cb)(this, transfer.source_node_id, (const char*)rsp.name.data, val)) {
1908 : // we want the parameter to be set with val
1909 0 : param_getset_req.index = 0;
1910 0 : memcpy(param_getset_req.name.data, rsp.name.data, rsp.name.len);
1911 0 : memcpy(¶m_getset_req.value.string_value, &val, sizeof(val));
1912 0 : param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE;
1913 0 : param_request_sent = false;
1914 0 : param_request_sent_ms = AP_HAL::millis();
1915 0 : param_request_node_id = transfer.source_node_id;
1916 0 : return;
1917 : }
1918 : }
1919 :
1920 0 : param_request_sent_ms = 0;
1921 0 : param_int_cb = nullptr;
1922 0 : param_float_cb = nullptr;
1923 0 : param_string_cb = nullptr;
1924 0 : }
1925 :
1926 21794 : void AP_DroneCAN::send_parameter_save_request()
1927 : {
1928 21794 : WITH_SEMAPHORE(_param_save_sem);
1929 21794 : if (param_save_request_sent) {
1930 21794 : return;
1931 : }
1932 0 : param_save_client.request(param_save_request_node_id, param_save_req);
1933 0 : param_save_request_sent = true;
1934 21794 : }
1935 :
1936 0 : bool AP_DroneCAN::save_parameters_on_node(uint8_t node_id, ParamSaveCb *cb)
1937 : {
1938 0 : WITH_SEMAPHORE(_param_save_sem);
1939 0 : if (save_param_cb != nullptr) {
1940 : //busy
1941 0 : return false;
1942 : }
1943 :
1944 0 : param_save_req.opcode = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_SAVE;
1945 0 : param_save_request_sent = false;
1946 0 : param_save_request_node_id = node_id;
1947 0 : save_param_cb = cb;
1948 0 : return true;
1949 0 : }
1950 :
1951 : // handle parameter save request response
1952 0 : void AP_DroneCAN::handle_param_save_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_ExecuteOpcodeResponse& rsp)
1953 : {
1954 0 : WITH_SEMAPHORE(_param_save_sem);
1955 0 : if (!save_param_cb) {
1956 0 : return;
1957 : }
1958 0 : (*save_param_cb)(this, transfer.source_node_id, rsp.ok);
1959 0 : save_param_cb = nullptr;
1960 0 : }
1961 :
1962 : // Send Reboot command
1963 : // Note: Do not call this from outside DroneCAN thread context,
1964 : // THIS IS NOT A THREAD SAFE API!
1965 0 : void AP_DroneCAN::send_reboot_request(uint8_t node_id)
1966 : {
1967 : uavcan_protocol_RestartNodeRequest request;
1968 0 : request.magic_number = UAVCAN_PROTOCOL_RESTARTNODE_REQUEST_MAGIC_NUMBER;
1969 0 : restart_node_client.request(node_id, request);
1970 0 : }
1971 :
1972 : // check if a option is set and if it is then reset it to 0.
1973 : // return true if it was set
1974 5 : bool AP_DroneCAN::check_and_reset_option(Options option)
1975 : {
1976 5 : bool ret = option_is_set(option);
1977 5 : if (ret) {
1978 0 : _options.set_and_save(int16_t(_options.get() & ~uint16_t(option)));
1979 : }
1980 5 : return ret;
1981 : }
1982 :
1983 : // handle prearm check
1984 200 : bool AP_DroneCAN::prearm_check(char* fail_msg, uint8_t fail_msg_len) const
1985 : {
1986 : // forward this to DNA_Server
1987 200 : return _dna_server.prearm_check(fail_msg, fail_msg_len);
1988 : }
1989 :
1990 : /*
1991 : periodic logging
1992 : */
1993 21794 : void AP_DroneCAN::logging(void)
1994 : {
1995 : #if HAL_LOGGING_ENABLED
1996 21794 : const uint32_t now_ms = AP_HAL::millis();
1997 21794 : if (now_ms - last_log_ms < 1000) {
1998 21314 : return;
1999 : }
2000 480 : last_log_ms = now_ms;
2001 480 : if (HAL_NUM_CAN_IFACES <= _driver_index) {
2002 : // no interface?
2003 0 : return;
2004 : }
2005 480 : const auto *iface = hal.can[_driver_index];
2006 480 : if (iface == nullptr) {
2007 0 : return;
2008 : }
2009 480 : const auto *stats = iface->get_statistics();
2010 480 : if (stats == nullptr) {
2011 : // statistics not implemented on this interface
2012 0 : return;
2013 : }
2014 480 : const auto &s = *stats;
2015 :
2016 : // @LoggerMessage: CANS
2017 : // @Description: CAN Bus Statistics
2018 : // @Field: TimeUS: Time since system startup
2019 : // @Field: I: driver index
2020 : // @Field: T: transmit success count
2021 : // @Field: Trq: transmit request count
2022 : // @Field: Trej: transmit reject count
2023 : // @Field: Tov: transmit overflow count
2024 : // @Field: Tto: transmit timeout count
2025 : // @Field: Tab: transmit abort count
2026 : // @Field: R: receive count
2027 : // @Field: Rov: receive overflow count
2028 : // @Field: Rer: receive error count
2029 : // @Field: Bo: bus offset error count
2030 : // @Field: Etx: ESC successful send count
2031 : // @Field: Stx: Servo successful send count
2032 : // @Field: Ftx: ESC/Servo failed-to-send count
2033 960 : AP::logger().WriteStreaming("CANS",
2034 : "TimeUS,I,T,Trq,Trej,Tov,Tto,Tab,R,Rov,Rer,Bo,Etx,Stx,Ftx",
2035 : "s#-------------",
2036 : "F--------------",
2037 : "QBIIIIIIIIIIIII",
2038 : AP_HAL::micros64(),
2039 480 : _driver_index,
2040 480 : s.tx_success,
2041 480 : s.tx_requests,
2042 480 : s.tx_rejected,
2043 480 : s.tx_overflow,
2044 480 : s.tx_timedout,
2045 480 : s.tx_abort,
2046 480 : s.rx_received,
2047 480 : s.rx_overflow,
2048 480 : s.rx_errors,
2049 480 : s.num_busoff_err,
2050 : _esc_send_count,
2051 : _srv_send_count,
2052 : _fail_send_count);
2053 : #endif // HAL_LOGGING_ENABLED
2054 : }
2055 :
2056 : // add an 11 bit auxillary driver
2057 0 : bool AP_DroneCAN::add_11bit_driver(CANSensor *sensor)
2058 : {
2059 0 : return canard_iface.add_11bit_driver(sensor);
2060 : }
2061 :
2062 : // handler for outgoing frames for auxillary drivers
2063 0 : bool AP_DroneCAN::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us)
2064 : {
2065 0 : if (out_frame.isExtended() && !option_is_set(Options::ALLOW_EXTENDED_AUX)) {
2066 : // don't allow extended frames to be sent by auxillary driver unless
2067 : // the user has specifically allowed it
2068 0 : return false;
2069 : }
2070 0 : return canard_iface.write_aux_frame(out_frame, timeout_us);
2071 : }
2072 :
2073 : #endif // HAL_ENABLE_DRONECAN_DRIVERS
|
