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: Oliver Walters / Currawong Engineering Pty Ltd
16 : */
17 :
18 :
19 : #include "AP_PiccoloCAN_config.h"
20 :
21 : #if AP_PICCOLOCAN_ENABLED
22 :
23 : #include <AP_HAL/AP_HAL.h>
24 : #include <AP_AHRS/AP_AHRS.h>
25 :
26 : #include "AP_PiccoloCAN.h"
27 :
28 : #include <AP_Param/AP_Param.h>
29 : #include <AP_BoardConfig/AP_BoardConfig.h>
30 : #include <AP_CANManager/AP_CANManager.h>
31 : #include <AP_Common/AP_Common.h>
32 : #include <AP_Scheduler/AP_Scheduler.h>
33 : #include <AP_HAL/utility/sparse-endian.h>
34 : #include <SRV_Channel/SRV_Channel.h>
35 : #include <GCS_MAVLink/GCS.h>
36 : #include <AP_CANManager/AP_CANManager.h>
37 :
38 : #include <AP_EFI/AP_EFI_Currawong_ECU.h>
39 : #include <AP_Generator/AP_Generator_Cortex.h>
40 : #include <AP_Servo_Telem/AP_Servo_Telem.h>
41 :
42 : #include <stdio.h>
43 :
44 : // Protocol files for the Velocity ESC
45 : #include <AP_PiccoloCAN/piccolo_protocol/ESCVelocityProtocol.h>
46 : #include <AP_PiccoloCAN/piccolo_protocol/ESCPackets.h>
47 :
48 : // Protocol files for the CBS servo
49 : #include <AP_PiccoloCAN/piccolo_protocol/ServoProtocol.h>
50 : #include <AP_PiccoloCAN/piccolo_protocol/ServoPackets.h>
51 :
52 : extern const AP_HAL::HAL& hal;
53 :
54 : // table of user-configurable Piccolo CAN bus parameters
55 : const AP_Param::GroupInfo AP_PiccoloCAN::var_info[] = {
56 :
57 : // @Param: ESC_BM
58 : // @DisplayName: ESC channels
59 : // @Description: Bitmask defining which ESC (motor) channels are to be transmitted over Piccolo CAN
60 : // @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
61 : // @User: Advanced
62 : AP_GROUPINFO("ESC_BM", 1, AP_PiccoloCAN, _esc_bm, 0xFFFF),
63 :
64 : // @Param: ESC_RT
65 : // @DisplayName: ESC output rate
66 : // @Description: Output rate of ESC command messages
67 : // @Units: Hz
68 : // @User: Advanced
69 : // @Range: 1 500
70 : AP_GROUPINFO("ESC_RT", 2, AP_PiccoloCAN, _esc_hz, PICCOLO_MSG_RATE_HZ_DEFAULT),
71 :
72 : // @Param: SRV_BM
73 : // @DisplayName: Servo channels
74 : // @Description: Bitmask defining which servo channels are to be transmitted over Piccolo CAN
75 : // @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
76 : // @User: Advanced
77 : AP_GROUPINFO("SRV_BM", 3, AP_PiccoloCAN, _srv_bm, 0xFFFF),
78 :
79 : // @Param: SRV_RT
80 : // @DisplayName: Servo command output rate
81 : // @Description: Output rate of servo command messages
82 : // @Units: Hz
83 : // @User: Advanced
84 : // @Range: 1 500
85 : AP_GROUPINFO("SRV_RT", 4, AP_PiccoloCAN, _srv_hz, PICCOLO_MSG_RATE_HZ_DEFAULT),
86 : #if AP_EFI_CURRAWONG_ECU_ENABLED
87 : // @Param: ECU_ID
88 : // @DisplayName: ECU Node ID
89 : // @Description: Node ID to send ECU throttle messages to. Set to zero to disable ECU throttle messages. Set to 255 to broadcast to all ECUs.
90 : // @Range: 0 255
91 : // @User: Advanced
92 : AP_GROUPINFO("ECU_ID", 5, AP_PiccoloCAN, _ecu_id, PICCOLO_CAN_ECU_ID_DEFAULT),
93 :
94 : // @Param: ECU_RT
95 : // @DisplayName: ECU command output rate
96 : // @Description: Output rate of ECU command messages
97 : // @Units: Hz
98 : // @User: Advanced
99 : // @Range: 1 500
100 : AP_GROUPINFO("ECU_RT", 6, AP_PiccoloCAN, _ecu_hz, PICCOLO_MSG_RATE_HZ_DEFAULT),
101 : #endif
102 : AP_GROUPEND
103 : };
104 :
105 0 : AP_PiccoloCAN::AP_PiccoloCAN()
106 : {
107 0 : AP_Param::setup_object_defaults(this, var_info);
108 0 : }
109 :
110 0 : AP_PiccoloCAN *AP_PiccoloCAN::get_pcan(uint8_t driver_index)
111 : {
112 0 : if (driver_index >= AP::can().get_num_drivers() ||
113 0 : AP::can().get_driver_type(driver_index) != AP_CAN::Protocol::PiccoloCAN) {
114 0 : return nullptr;
115 : }
116 :
117 0 : return static_cast<AP_PiccoloCAN*>(AP::can().get_driver(driver_index));
118 : }
119 :
120 0 : bool AP_PiccoloCAN::add_interface(AP_HAL::CANIface* can_iface) {
121 0 : if (_can_iface != nullptr) {
122 0 : return false;
123 : }
124 :
125 0 : _can_iface = can_iface;
126 :
127 0 : if (_can_iface == nullptr) {
128 0 : return false;
129 : }
130 :
131 0 : if (!_can_iface->is_initialized()) {
132 0 : return false;
133 : }
134 :
135 0 : if (!_can_iface->set_event_handle(&sem_handle)) {
136 0 : return false;
137 : }
138 0 : return true;
139 : }
140 :
141 : // initialize PiccoloCAN bus
142 0 : void AP_PiccoloCAN::init(uint8_t driver_index)
143 : {
144 0 : _driver_index = driver_index;
145 :
146 0 : if (_initialized) {
147 0 : return;
148 : }
149 : // start calls to loop in separate thread
150 0 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_PiccoloCAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_MAIN, 1)) {
151 0 : return;
152 : }
153 :
154 0 : _initialized = true;
155 :
156 0 : snprintf(_thread_name, sizeof(_thread_name), "PiccoloCAN_%u", driver_index);
157 : }
158 :
159 : // loop to send output to CAN devices in background thread
160 0 : void AP_PiccoloCAN::loop()
161 : {
162 0 : AP_HAL::CANFrame txFrame {};
163 0 : AP_HAL::CANFrame rxFrame {};
164 :
165 0 : uint16_t esc_tx_counter = 0;
166 0 : uint16_t servo_tx_counter = 0;
167 : #if AP_EFI_CURRAWONG_ECU_ENABLED
168 0 : uint16_t ecu_tx_counter = 0;
169 : #endif
170 :
171 : // CAN Frame ID components
172 : uint8_t frame_id_group; // Piccolo message group
173 : uint16_t frame_id_device; // Device identifier
174 :
175 : while (true) {
176 :
177 0 : if (!_initialized) {
178 0 : hal.scheduler->delay_microseconds(10000);
179 0 : continue;
180 : }
181 :
182 : // Calculate the output rate for ESC commands
183 0 : _esc_hz.set(constrain_int16(_esc_hz, PICCOLO_MSG_RATE_HZ_MIN, PICCOLO_MSG_RATE_HZ_MAX));
184 :
185 0 : uint16_t escCmdRateMs = 1000 / _esc_hz;
186 :
187 : // Calculate the output rate for servo commands
188 0 : _srv_hz.set(constrain_int16(_srv_hz, PICCOLO_MSG_RATE_HZ_MIN, PICCOLO_MSG_RATE_HZ_MAX));
189 :
190 0 : uint16_t servoCmdRateMs = 1000 / _srv_hz;
191 : #if AP_EFI_CURRAWONG_ECU_ENABLED
192 0 : _ecu_hz.set(constrain_int16(_ecu_hz, PICCOLO_MSG_RATE_HZ_MIN, PICCOLO_MSG_RATE_HZ_MAX));
193 :
194 0 : uint16_t ecuCmdRateMs = 1000 / _ecu_hz;
195 : #endif
196 :
197 : // 1ms loop delay
198 0 : hal.scheduler->delay_microseconds(1000);
199 :
200 : // Transmit ESC commands at regular intervals
201 0 : if (esc_tx_counter++ > escCmdRateMs) {
202 0 : esc_tx_counter = 0;
203 0 : send_esc_messages();
204 : }
205 :
206 : // Transmit servo commands at regular intervals
207 0 : if (servo_tx_counter++ > servoCmdRateMs) {
208 0 : servo_tx_counter = 0;
209 0 : send_servo_messages();
210 : }
211 :
212 : #if AP_EFI_CURRAWONG_ECU_ENABLED
213 : // Transmit ecu throttle commands at regular intervals
214 0 : if (ecu_tx_counter++ > ecuCmdRateMs) {
215 0 : ecu_tx_counter = 0;
216 0 : send_ecu_messages();
217 : }
218 : #endif
219 :
220 : // Look for any message responses on the CAN bus
221 0 : while (read_frame(rxFrame, 0)) {
222 :
223 : // Extract group and device ID values from the frame identifier
224 0 : frame_id_group = (rxFrame.id >> 24) & 0x1F;
225 0 : frame_id_device = (rxFrame.id >> 8) & 0xFF;
226 :
227 : // Only accept extended messages
228 0 : if ((rxFrame.id & AP_HAL::CANFrame::FlagEFF) == 0) {
229 0 : continue;
230 : }
231 :
232 0 : switch (PiccoloCAN_MessageGroup(frame_id_group)) {
233 : // ESC messages exist in the ACTUATOR group
234 0 : case PiccoloCAN_MessageGroup::ACTUATOR:
235 :
236 0 : switch (PiccoloCAN_DeviceType(frame_id_device)) {
237 0 : case PiccoloCAN_DeviceType::SERVO:
238 0 : handle_servo_message(rxFrame);
239 0 : break;
240 0 : case PiccoloCAN_DeviceType::ESC:
241 0 : handle_esc_message(rxFrame);
242 0 : break;
243 0 : default:
244 : // Unknown actuator type
245 0 : break;
246 : }
247 :
248 0 : break;
249 0 : case PiccoloCAN_MessageGroup::ECU_OUT:
250 : #if AP_EFI_CURRAWONG_ECU_ENABLED
251 0 : handle_ecu_message(rxFrame);
252 : #endif
253 0 : break;
254 0 : case PiccoloCAN_MessageGroup::BATTERY:
255 0 : handle_cortex_message(rxFrame);
256 0 : break;
257 0 : default:
258 0 : break;
259 : }
260 : }
261 0 : }
262 : }
263 :
264 : // write frame on CAN bus, returns true on success
265 0 : bool AP_PiccoloCAN::write_frame(AP_HAL::CANFrame &out_frame, uint32_t timeout_us)
266 : {
267 0 : if (!_initialized) {
268 0 : return false;
269 : }
270 :
271 0 : bool read_select = false;
272 0 : bool write_select = true;
273 0 : const uint64_t deadline_us = AP_HAL::micros64() + timeout_us;
274 0 : bool ret = _can_iface->select(read_select, write_select, &out_frame, deadline_us);
275 :
276 0 : if (!ret || !write_select) {
277 0 : return false;
278 : }
279 :
280 0 : return (_can_iface->send(out_frame, deadline_us, AP_HAL::CANIface::AbortOnError) == 1);
281 : }
282 :
283 : // read frame on CAN bus, returns true on succses
284 0 : bool AP_PiccoloCAN::read_frame(AP_HAL::CANFrame &recv_frame, uint32_t timeout_us)
285 : {
286 0 : if (!_initialized) {
287 0 : return false;
288 : }
289 :
290 0 : bool read_select = true;
291 0 : bool write_select = false;
292 0 : bool ret = _can_iface->select(read_select, write_select, nullptr, AP_HAL::micros64() + timeout_us);
293 :
294 0 : if (!ret || !read_select) {
295 : // No frame available
296 0 : return false;
297 : }
298 :
299 : uint64_t time;
300 0 : AP_HAL::CANIface::CanIOFlags flags {};
301 :
302 0 : return (_can_iface->receive(recv_frame, time, flags) == 1);
303 : }
304 :
305 : // called from SRV_Channels
306 0 : void AP_PiccoloCAN::update()
307 : {
308 : /* Read out the servo commands from the channel mixer */
309 0 : for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_SERVO; ii++) {
310 :
311 0 : if (is_servo_channel_active(ii)) {
312 :
313 0 : uint16_t output = 0;
314 :
315 0 : SRV_Channel::Function function = SRV_Channels::channel_function(ii);
316 :
317 0 : if (SRV_Channels::get_output_pwm(function, output)) {
318 0 : _servos[ii].command = output;
319 0 : _servos[ii].newCommand = true;
320 : }
321 : }
322 : }
323 :
324 : /* Read out the ESC commands from the channel mixer */
325 0 : for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_ESC; ii++) {
326 :
327 0 : if (is_esc_channel_active(ii)) {
328 :
329 0 : uint16_t output = 0;
330 :
331 0 : SRV_Channel::Function motor_function = SRV_Channels::get_motor_function(ii);
332 :
333 0 : if (SRV_Channels::get_output_pwm(motor_function, output)) {
334 0 : _escs[ii].command = output;
335 0 : _escs[ii].newCommand = true;
336 : }
337 : }
338 : }
339 :
340 : #if AP_EFI_CURRAWONG_ECU_ENABLED
341 0 : if (_ecu_id != 0) {
342 0 : _ecu_info.command = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
343 0 : _ecu_info.newCommand = true;
344 : }
345 : #endif // AP_EFI_CURRAWONG_ECU_ENABLED
346 :
347 : #if AP_SERVO_TELEM_ENABLED
348 0 : AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
349 0 : if (servo_telem != nullptr) {
350 0 : for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_SERVO; ii++) {
351 0 : AP_PiccoloCAN_Servo &servo = _servos[ii];
352 0 : if (servo.newTelemetry) {
353 : union {
354 : Servo_ErrorBits_t ebits;
355 : uint8_t errors;
356 : } err;
357 0 : err.ebits = servo.status.statusA.errors;
358 :
359 0 : const AP_Servo_Telem::TelemetryData telem_data {
360 0 : .command_position = servo.commandedPosition(),
361 0 : .measured_position = servo.position(),
362 0 : .speed = servo.speed(),
363 0 : .voltage = servo.voltage(),
364 0 : .current = servo.current(),
365 0 : .duty_cycle = servo.dutyCycle(),
366 0 : .motor_temperature_cdeg = int16_t(servo.temperature() * 100),
367 0 : .status_flags = err.errors,
368 : .present_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION |
369 : AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
370 : AP_Servo_Telem::TelemetryData::Types::SPEED |
371 : AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
372 : AP_Servo_Telem::TelemetryData::Types::CURRENT |
373 : AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE |
374 : AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP |
375 : AP_Servo_Telem::TelemetryData::Types::STATUS
376 0 : };
377 :
378 0 : servo_telem->update_telem_data(ii, telem_data);
379 :
380 0 : servo.newTelemetry = false;
381 : }
382 : }
383 : }
384 : #endif
385 0 : }
386 :
387 :
388 : // send servo messages over CAN
389 0 : void AP_PiccoloCAN::send_servo_messages(void)
390 : {
391 0 : AP_HAL::CANFrame txFrame {};
392 :
393 : // No servos are selected? Don't send anything!
394 0 : if (_srv_bm == 0x00) {
395 0 : return;
396 : }
397 :
398 0 : bool send_cmd = false;
399 0 : int16_t cmd[4] {};
400 : uint8_t idx;
401 :
402 : // Transmit bulk command packets to 4x servos simultaneously
403 0 : for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_GROUP_SERVO; ii++) {
404 :
405 0 : send_cmd = false;
406 :
407 0 : for (uint8_t jj = 0; jj < 4; jj++) {
408 :
409 0 : idx = (ii * 4) + jj;
410 :
411 : // Set default command value if an output field is unused
412 0 : cmd[jj] = 0x7FFF;
413 :
414 : // Skip servo if the output is not enabled
415 0 : if (!is_servo_channel_active(idx)) {
416 0 : continue;
417 : }
418 :
419 : /* Check if the servo is enabled.
420 : * If it is not enabled, send an enable message.
421 : */
422 :
423 0 : if (!is_servo_present(idx) || !is_servo_enabled(idx)) {
424 : // Servo is not enabled
425 0 : encodeServo_EnablePacket(&txFrame);
426 0 : txFrame.id |= (idx + 1);
427 0 : write_frame(txFrame, 1000);
428 0 : } else if (_servos[idx].newCommand) {
429 : // A new command is provided
430 0 : send_cmd = true;
431 0 : cmd[jj] = _servos[idx].command;
432 0 : _servos[idx].newCommand = false;
433 : }
434 : }
435 :
436 0 : if (send_cmd) {
437 0 : encodeServo_MultiPositionCommandPacket(
438 : &txFrame,
439 0 : cmd[0],
440 0 : cmd[1],
441 0 : cmd[2],
442 0 : cmd[3],
443 : (PKT_SERVO_MULTI_COMMAND_1 + ii)
444 : );
445 :
446 : // Broadcast the command to all servos
447 0 : txFrame.id |= 0xFF;
448 :
449 0 : write_frame(txFrame, 1000);
450 : }
451 : }
452 : }
453 :
454 :
455 : // send ESC messages over CAN
456 0 : void AP_PiccoloCAN::send_esc_messages(void)
457 : {
458 0 : AP_HAL::CANFrame txFrame {};
459 :
460 : // No ESCs are selected? Don't send anything
461 0 : if (_esc_bm == 0x00) {
462 0 : return;
463 : }
464 :
465 : // System is armed - send out ESC commands
466 0 : if (hal.util->get_soft_armed()) {
467 :
468 0 : bool send_cmd = false;
469 0 : int16_t cmd[4] {};
470 : uint8_t idx;
471 :
472 : // Transmit bulk command packets to 4x ESC simultaneously
473 0 : for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_GROUP_ESC; ii++) {
474 :
475 0 : send_cmd = false;
476 :
477 0 : for (uint8_t jj = 0; jj < 4; jj++) {
478 :
479 0 : idx = (ii * 4) + jj;
480 :
481 : // Set default command value if an output field is unused
482 0 : cmd[jj] = 0x7FFF;
483 :
484 : // Skip an ESC if the motor channel is not enabled
485 0 : if (!is_esc_channel_active(idx)) {
486 0 : continue;
487 : }
488 :
489 : /* Check if the ESC is software-inhibited.
490 : * If so, send a message to enable it.
491 : */
492 0 : if (is_esc_present(idx) && !is_esc_enabled(idx)) {
493 0 : encodeESC_EnablePacket(&txFrame);
494 0 : txFrame.id |= (idx + 1);
495 0 : write_frame(txFrame, 1000);
496 : }
497 0 : else if (_escs[idx].newCommand) {
498 0 : send_cmd = true;
499 0 : cmd[jj] = _escs[idx].command;
500 0 : _escs[idx].newCommand = false;
501 : } else {
502 : // A command of 0x7FFF is 'out of range' and will be ignored by the corresponding ESC
503 0 : cmd[jj] = 0x7FFF;
504 : }
505 : }
506 :
507 0 : if (send_cmd) {
508 0 : encodeESC_CommandMultipleESCsPacket(
509 : &txFrame,
510 0 : cmd[0],
511 0 : cmd[1],
512 0 : cmd[2],
513 0 : cmd[3],
514 : (PKT_ESC_SETPOINT_1 + ii)
515 : );
516 :
517 : // Broadcast the command to all ESCs
518 0 : txFrame.id |= 0xFF;
519 :
520 0 : write_frame(txFrame, 1000);
521 : }
522 : }
523 :
524 : } else {
525 : // System is NOT armed - send a "disable" message to all ESCs on the bus
526 :
527 : // Command all ESC into software disable mode
528 0 : encodeESC_DisablePacket(&txFrame);
529 :
530 : // Set the ESC address to the broadcast ID (0xFF)
531 0 : txFrame.id |= 0xFF;
532 :
533 0 : write_frame(txFrame, 1000);
534 : }
535 : }
536 :
537 :
538 : // interpret a servo message received over CAN
539 0 : bool AP_PiccoloCAN::handle_servo_message(AP_HAL::CANFrame &frame)
540 : {
541 : // The servo address is the lower byte of the frame ID
542 0 : uint8_t addr = frame.id & 0xFF;
543 :
544 : // Ignore servo with an invalid node ID
545 0 : if (addr == 0x00) {
546 0 : return false;
547 : }
548 :
549 : // Subtract to get the address in memory
550 0 : addr -= 1;
551 :
552 : // Maximum number of servos allowed
553 0 : if (addr >= PICCOLO_CAN_MAX_NUM_SERVO) {
554 0 : return false;
555 : }
556 :
557 : // Pass the CAN frame off to the specific servo
558 0 : return _servos[addr].handle_can_frame(frame);
559 : }
560 :
561 :
562 : // interpret an ESC message received over CAN
563 0 : bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame)
564 : {
565 : // The ESC address is the lower byte of the frame ID
566 0 : uint8_t addr = frame.id & 0xFF;
567 :
568 : // Ignore any ESC with node ID of zero
569 0 : if (addr == 0x00) {
570 0 : return false;
571 : }
572 :
573 : // Subtract to get the address in memory
574 0 : addr -= 1;
575 :
576 : // Maximum number of ESCs allowed
577 0 : if (addr >= PICCOLO_CAN_MAX_NUM_ESC) {
578 0 : return false;
579 : }
580 :
581 0 : return _escs[addr].handle_can_frame(frame);
582 : }
583 :
584 : #if AP_EFI_CURRAWONG_ECU_ENABLED
585 0 : void AP_PiccoloCAN::send_ecu_messages(void)
586 : {
587 0 : AP_HAL::CANFrame txFrame {};
588 :
589 : // No ECU node id set, don't send anything
590 0 : if (_ecu_id == 0) {
591 0 : return;
592 : }
593 :
594 0 : if (_ecu_info.newCommand) {
595 0 : encodeECU_ThrottleCommandPacket(&txFrame, _ecu_info.command);
596 0 : txFrame.id |= (uint8_t) _ecu_id;
597 :
598 0 : _ecu_info.newCommand = false;
599 :
600 0 : write_frame(txFrame, 1000);
601 : }
602 : }
603 :
604 0 : bool AP_PiccoloCAN::handle_ecu_message(AP_HAL::CANFrame &frame)
605 : {
606 : // Get the ecu instance
607 0 : AP_EFI_Currawong_ECU* ecu = AP_EFI_Currawong_ECU::get_instance();
608 0 : if (ecu != nullptr) {
609 0 : return ecu->handle_message(frame);
610 : }
611 0 : return false;
612 : }
613 : #endif // AP_EFI_CURRAWONG_ECU_ENABLED
614 :
615 :
616 0 : bool AP_PiccoloCAN::handle_cortex_message(AP_HAL::CANFrame &frame)
617 : {
618 : #if AP_GENERATOR_CORTEX_ENABLED
619 : // Get the generator instance
620 0 : AP_Generator_Cortex* gen = AP_Generator_Cortex::get_instance();
621 :
622 0 : if (gen != nullptr) {
623 0 : return gen->handle_message(frame, *this);
624 : }
625 : #endif // AP_GENERATOR_CORTEX_ENABLED
626 :
627 0 : return false;
628 : }
629 :
630 :
631 : /**
632 : * Check if a given servo channel is "active" (has been configured for Piccolo control output)
633 : */
634 0 : bool AP_PiccoloCAN::is_servo_channel_active(uint8_t chan)
635 : {
636 : // First check if the particular servo channel is enabled in the channel mask
637 0 : if (((_srv_bm >> chan) & 0x01) == 0x00) {
638 0 : return false;
639 : }
640 :
641 0 : SRV_Channel::Function function = SRV_Channels::channel_function(chan);
642 :
643 : // Ignore if the servo channel does not have a function assigned
644 0 : if (function <= SRV_Channel::k_none) {
645 0 : return false;
646 : }
647 :
648 : // Ignore if the assigned function is a motor function
649 0 : if (SRV_Channel::is_motor(function)) {
650 0 : return false;
651 : }
652 :
653 : // We can safely say that the particular servo channel is active
654 0 : return true;
655 : }
656 :
657 : /**
658 : * Check if a given ESC channel is "active" (has been configured for Piccolo control output)
659 : */
660 0 : bool AP_PiccoloCAN::is_esc_channel_active(uint8_t chan)
661 : {
662 : // First check if the particular ESC channel is enabled in the channel mask
663 0 : if (((_esc_bm >> chan) & 0x01) == 0x00) {
664 0 : return false;
665 : }
666 :
667 : // Check if a motor function is assigned for this motor channel
668 0 : SRV_Channel::Function motor_function = SRV_Channels::get_motor_function(chan);
669 :
670 0 : if (SRV_Channels::function_assigned(motor_function)) {
671 0 : return true;
672 : }
673 :
674 0 : return false;
675 : }
676 :
677 :
678 : /**
679 : * Determine if a servo is present on the CAN bus (has telemetry data been received)
680 : */
681 0 : bool AP_PiccoloCAN::is_servo_present(uint8_t chan, uint32_t timeout_us)
682 : {
683 0 : if (chan >= PICCOLO_CAN_MAX_NUM_SERVO) {
684 0 : return false;
685 : }
686 :
687 0 : return _servos[chan].is_connected(timeout_us);
688 : }
689 :
690 :
691 : /**
692 : * Determine if an ESC is present on the CAN bus (has telemetry data been received)
693 : */
694 0 : bool AP_PiccoloCAN::is_esc_present(uint8_t chan, uint32_t timeout_us)
695 : {
696 0 : if (chan >= PICCOLO_CAN_MAX_NUM_ESC) {
697 0 : return false;
698 : }
699 :
700 0 : return _escs[chan].is_connected(timeout_us);
701 : }
702 :
703 :
704 : /**
705 : * Check if a given servo is enabled
706 : */
707 0 : bool AP_PiccoloCAN::is_servo_enabled(uint8_t chan)
708 : {
709 0 : if (chan >= PICCOLO_CAN_MAX_NUM_SERVO) {
710 0 : return false;
711 : }
712 :
713 : // If the servo is not present, we cannot determine if it is enabled or not
714 0 : if (!is_servo_present(chan)) {
715 0 : return false;
716 : }
717 :
718 0 : return _servos[chan].is_enabled();
719 : }
720 :
721 :
722 : /**
723 : * Check if a given ESC is enabled (both hardware and software enable flags)
724 : */
725 0 : bool AP_PiccoloCAN::is_esc_enabled(uint8_t chan)
726 : {
727 0 : if (chan >= PICCOLO_CAN_MAX_NUM_ESC) {
728 0 : return false;
729 : }
730 :
731 : // If the ESC is not present, we cannot determine if it is enabled or not
732 0 : if (!is_esc_present(chan)) {
733 0 : return false;
734 : }
735 :
736 0 : return _escs[chan].is_enabled();
737 : }
738 :
739 :
740 0 : bool AP_PiccoloCAN::pre_arm_check(char* reason, uint8_t reason_len)
741 : {
742 : // Check that each required servo is present on the bus
743 0 : for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_SERVO; ii++) {
744 :
745 0 : if (is_servo_channel_active(ii)) {
746 :
747 0 : if (!is_servo_present(ii)) {
748 0 : snprintf(reason, reason_len, "Servo %u not detected", ii + 1);
749 0 : return false;
750 : }
751 : }
752 : }
753 :
754 : // Check that each required ESC is present on the bus
755 0 : for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_ESC; ii++) {
756 :
757 : // Skip any ESC channels where the motor channel is not enabled
758 0 : if (is_esc_channel_active(ii)) {
759 :
760 0 : if (!is_esc_present(ii)) {
761 0 : snprintf(reason, reason_len, "ESC %u not detected", ii + 1);
762 0 : return false;
763 : }
764 :
765 0 : if (_escs[ii].is_hw_inhibited()) {
766 0 : snprintf(reason, reason_len, "ESC %u is hardware inhibited", (ii + 1));
767 0 : return false;
768 : }
769 : }
770 : }
771 :
772 0 : return true;
773 : }
774 :
775 :
776 : #endif // AP_PICCOLOCAN_ENABLED
|
