diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index 75be8aafff..24bf2cc7ba 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp @@ -33,6 +33,8 @@ #include #include +#include + #include // Protocol files for the Velocity ESC @@ -233,6 +235,13 @@ void AP_PiccoloCAN::loop() break; } + break; + case MessageGroup::ECU_OUT: + #if HAL_EFI_CURRAWONG_ECU_ENABLED + if (handle_ecu_message(rxFrame)) { + // Returns true if the message was successfully decoded + } + #endif break; default: break; @@ -736,6 +745,18 @@ bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame) return result; } +#if HAL_EFI_CURRAWONG_ECU_ENABLED +bool AP_PiccoloCAN::handle_ecu_message(AP_HAL::CANFrame &frame) +{ + // Get the ecu instance + AP_EFI_Currawong_ECU* ecu = AP_EFI_Currawong_ECU::get_instance(); + if (ecu != nullptr) { + return ecu->handle_message(frame); + } + return false; +} +#endif + /** * Check if a given servo channel is "active" (has been configured for Piccolo control output) */ diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h index d8c1e83038..3ee3411784 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h @@ -28,6 +28,8 @@ #include "piccolo_protocol/ServoPackets.h" +#include + // maximum number of ESC allowed on CAN bus simultaneously #define PICCOLO_CAN_MAX_NUM_ESC 16 #define PICCOLO_CAN_MAX_GROUP_ESC (PICCOLO_CAN_MAX_NUM_ESC / 4) @@ -130,6 +132,11 @@ private: // interpret a servo message received over CAN bool handle_servo_message(AP_HAL::CANFrame &frame); + +#if HAL_EFI_CURRAWONG_ECU_ENABLED + // interpret an ECU message received over CAN + bool handle_ecu_message(AP_HAL::CANFrame &frame); +#endif bool _initialized; char _thread_name[16]; @@ -207,6 +214,8 @@ private: AP_Int32 _srv_bm; //! Servo selection bitmask AP_Int16 _srv_hz; //! Servo update rate (Hz) + AP_Int8 _ecu_en; //! ECU Enable + HAL_Semaphore _telem_sem; };