diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index 24bf2cc7ba..2cabbbb8db 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp @@ -45,6 +45,10 @@ #include #include +// Protocol files for the ECU +#include +#include + extern const AP_HAL::HAL& hal; #if HAL_CANMANAGER_ENABLED @@ -85,7 +89,22 @@ const AP_Param::GroupInfo AP_PiccoloCAN::var_info[] = { // @User: Advanced // @Range: 1 500 AP_GROUPINFO("SRV_RT", 4, AP_PiccoloCAN, _srv_hz, PICCOLO_MSG_RATE_HZ_DEFAULT), +#if HAL_EFI_CURRAWONG_ECU_ENABLED + // @Param: ECU_ID + // @DisplayName: ECU Node ID + // @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. + // @Range: 0 255 + // @User: Advanced + AP_GROUPINFO("ECU_ID", 5, AP_PiccoloCAN, _ecu_id, PICCOLO_CAN_ECU_ID_DEFAULT), + // @Param: ECU_RT + // @DisplayName: ECU command output rate + // @Description: Output rate of ECU command messages + // @Units: Hz + // @User: Advanced + // @Range: 1 500 + AP_GROUPINFO("ECU_RT", 6, AP_PiccoloCAN, _ecu_hz, PICCOLO_MSG_RATE_HZ_DEFAULT), +#endif AP_GROUPEND }; @@ -163,6 +182,9 @@ void AP_PiccoloCAN::loop() uint16_t esc_tx_counter = 0; uint16_t servo_tx_counter = 0; +#if HAL_EFI_CURRAWONG_ECU_ENABLED + uint16_t ecu_tx_counter = 0; +#endif // CAN Frame ID components uint8_t frame_id_group; // Piccolo message group @@ -185,7 +207,11 @@ void AP_PiccoloCAN::loop() _srv_hz.set(constrain_int16(_srv_hz, PICCOLO_MSG_RATE_HZ_MIN, PICCOLO_MSG_RATE_HZ_MAX)); uint16_t servoCmdRateMs = 1000 / _srv_hz; +#if HAL_EFI_CURRAWONG_ECU_ENABLED + _ecu_hz = constrain_int16(_ecu_hz, PICCOLO_MSG_RATE_HZ_MIN, PICCOLO_MSG_RATE_HZ_MAX); + uint16_t ecuCmdRateMs = 1000 / _ecu_hz; +#endif uint64_t timeout = AP_HAL::micros64() + 250ULL; // 1ms loop delay @@ -203,6 +229,14 @@ void AP_PiccoloCAN::loop() send_servo_messages(); } +#if HAL_EFI_CURRAWONG_ECU_ENABLED + // Transmit ecu throttle commands at regular intervals + if (ecu_tx_counter++ > ecuCmdRateMs) { + ecu_tx_counter = 0; + send_ecu_messages(); + } +#endif + // Look for any message responses on the CAN bus while (read_frame(rxFrame, timeout)) { @@ -330,6 +364,13 @@ void AP_PiccoloCAN::update() } } +#if HAL_EFI_CURRAWONG_ECU_ENABLED + if (_ecu_id != 0) { + _ecu_info.command = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); + _ecu_info.newCommand = true; + } +#endif // HAL_EFI_CURRAWONG_ECU_ENABLED + AP_Logger *logger = AP_Logger::get_singleton(); // Push received telemetry data into the logging system @@ -746,6 +787,27 @@ bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame) } #if HAL_EFI_CURRAWONG_ECU_ENABLED +void AP_PiccoloCAN::send_ecu_messages(void) +{ + AP_HAL::CANFrame txFrame {}; + + const uint64_t timeout = AP_HAL::micros64() + 1000ULL; + + // No ECU node id set, don't send anything + if (_ecu_id == 0) { + return; + } + + if (_ecu_info.newCommand) { + encodeECU_ThrottleCommandPacket(&txFrame, _ecu_info.command); + txFrame.id |= (uint8_t) _ecu_id; + + _ecu_info.newCommand = false; + + write_frame(txFrame, timeout); + } +} + bool AP_PiccoloCAN::handle_ecu_message(AP_HAL::CANFrame &frame) { // Get the ecu instance @@ -755,7 +817,7 @@ bool AP_PiccoloCAN::handle_ecu_message(AP_HAL::CANFrame &frame) } return false; } -#endif +#endif // HAL_EFI_CURRAWONG_ECU_ENABLED /** * 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 3ee3411784..d18c7a1e50 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h @@ -12,7 +12,7 @@ * You should have received a copy of the GNU General Public License along * with this program. If not, see . * - * Author: Oliver Walters + * Author: Oliver Walters / Currawong Engineering Pty Ltd */ #pragma once @@ -47,6 +47,8 @@ #define PICCOLO_MSG_RATE_HZ_MAX 500 #define PICCOLO_MSG_RATE_HZ_DEFAULT 50 +#define PICCOLO_CAN_ECU_ID_DEFAULT 0 + class AP_PiccoloCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { public: @@ -134,6 +136,8 @@ private: bool handle_servo_message(AP_HAL::CANFrame &frame); #if HAL_EFI_CURRAWONG_ECU_ENABLED + void send_ecu_messages(void); + // interpret an ECU message received over CAN bool handle_ecu_message(AP_HAL::CANFrame &frame); #endif @@ -207,6 +211,11 @@ private: } _esc_info[PICCOLO_CAN_MAX_NUM_ESC]; + struct CurrawongECU_Info_t { + float command; + bool newCommand; + } _ecu_info; + // Piccolo CAN parameters AP_Int32 _esc_bm; //! ESC selection bitmask AP_Int16 _esc_hz; //! ESC update rate (Hz) @@ -214,7 +223,8 @@ private: AP_Int32 _srv_bm; //! Servo selection bitmask AP_Int16 _srv_hz; //! Servo update rate (Hz) - AP_Int8 _ecu_en; //! ECU Enable + AP_Int16 _ecu_id; //! ECU Node ID + AP_Int16 _ecu_hz; //! ECU update rate (Hz) HAL_Semaphore _telem_sem; };