AP_PiccoloCAN: SendECU throttle commands over CAN

libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp

@@ -45,6 +45,10 @@
 #include <AP_PiccoloCAN/piccolo_protocol/ServoProtocol.h>
 #include <AP_PiccoloCAN/piccolo_protocol/ServoPackets.h>
 
+// Protocol files for the ECU
+#include <AP_PiccoloCAN/piccolo_protocol/ECUProtocol.h>
+#include <AP_PiccoloCAN/piccolo_protocol/ECUPackets.h>
+
 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)

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 <http://www.gnu.org/licenses/>.
  *
- * 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;
 };