AP_ICEngine : Fuel pump implementation and cleanup

CarbonixUAV · Dec 10, 2024 · a68c66f · a68c66f
1 parent d395812
commit a68c66f
Show file tree

Hide file tree

Showing 3 changed files with 47 additions and 31 deletions.
diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp
@@ -199,6 +199,10 @@ AP_ICEngine::AP_ICEngine()
     // ICEngine runs at 10Hz
     _rpm_filter.set_cutoff_frequency(10, 0.5f);
 #endif
+
+#if AP_ICENGINE_TCA9554_STARTER_ENABLED
+    tca9554_starter.TCA9554_init();
+#endif
 }
 
 /*

diff --git a/libraries/AP_ICEngine/AP_ICEngine_TCA9554.cpp b/libraries/AP_ICEngine/AP_ICEngine_TCA9554.cpp
@@ -11,30 +11,38 @@ extern const AP_HAL::HAL& hal;
 
 /*
  * TCA9554 output register mapping for PMB Rev E
- * P0 = PMU_EN - PMU output ON/OFF  (CN6 pin 2)
- * P1 = ECU_EN - Unused (previously Engine Kill Switch)
+ * P0 = PMU_EN - Unused 
+ * P1 = ECU_EN - Fuel pump MOSFET controller
  * P2 = I2C_P2 - Unused
  * P3 = LED (active low)
- * P4 = PMU_START - Crank Direction (CN6 pin 5)
+ * P4 = PMU_START - Unused
  * P5 = PMU_ARM  - Crank Signal (CN6 pin 6)
  * P6 = PMU_STAT_IN - Unused
  * P7 = PMU_STAT - Unused
  */
 #define TCA9554_I2C_BUS      1
 #define TCA9554_I2C_ADDR     0x20
 #define TCA9554_OUTPUT       0x01  // Output Port register address. Outgoing logic levels
-#define TCA9554_OUT_DEFAULT  0x30  // 0011 0000
+#define TCA9554_OUT_DEFAULT  0x20  // 0010 0000
+//#define TCA9554_OUT_DEFAULT  0x22  // 0010 0010
 #define TCA9554_CONF         0x03  // Configuration Port register address [0 = Output]
-#define TCA9554_PINS         0xC2  // Set all used ports to outputs = 1100 0010
+#define TCA9554_PINS         0xCD  // Set P1 and P5 as output [11001101]. P4 - direction (if required)
+
+#define ECU_EN 0x02
+#define PMU_ARM 0x20
+#define FORWARD_DIRECTION 0x10
+
+#define FUEL_PRIME_DURATION 30000
 
 /*
   initialise TCA9554
  */
 bool AP_ICEngine_TCA9554::TCA9554_init()
 {
+    initialised = false;
     dev_TCA9554 = std::move(hal.i2c_mgr->get_device(TCA9554_I2C_BUS, TCA9554_I2C_ADDR));
     if (!dev_TCA9554) {
-        return false;
+        return initialised;
     }
     WITH_SEMAPHORE(dev_TCA9554->get_semaphore());
 
@@ -46,21 +54,25 @@ bool AP_ICEngine_TCA9554::TCA9554_init()
     // set outputs
     bool ret = dev_TCA9554->write_register(TCA9554_OUTPUT, TCA9554_OUT_DEFAULT);
     if (!ret) {
-        return false;
+        return initialised;
     }
     ret = dev_TCA9554->write_register(TCA9554_CONF, TCA9554_PINS, true);
     if (!ret) {
-        return false;
+        return initialised;
     }
 
     dev_TCA9554->set_retries(1);
-    return true;
+
+    fuel_prime_count = AP_HAL::millis();
+
+    initialised = true;
+    return initialised;
 }
 
 /*
   set the state of the i2c controller
  */
-void AP_ICEngine_TCA9554::TCA9554_set(TCA9554_state_t value)
+void AP_ICEngine_TCA9554::TCA9554_set(uint8_t value)
 {
     const uint32_t now_ms = AP_HAL::millis();
     if (now_ms - last_reg_check_ms > 100) {
@@ -73,29 +85,31 @@ void AP_ICEngine_TCA9554::TCA9554_set(TCA9554_state_t value)
         dev_TCA9554->check_next_register();
     }
 
-    if (value != last_state) {
+    if (value != prev_value) {
         WITH_SEMAPHORE(dev_TCA9554->get_semaphore());
-        // set outputs and status leds
-        if (dev_TCA9554->write_register(TCA9554_OUTPUT, (~(value<<2) & 0x0C) | value)) {
-            last_state = value;
+        // set outputs
+        if (dev_TCA9554->write_register(TCA9554_OUTPUT, value)) {
+            prev_value = value;
         }
     }
 }
 
 void AP_ICEngine_TCA9554::set_starter(bool on, AP_Int8 crank_direction)
 {
-    if (!initialised) {
-        initialised = TCA9554_init();
-        if (!initialised) {
-            // waiting for power to PMU
-            return;
+    uint8_t value = 0;
+
+    if (on) {
+        value = ECU_EN;
+        if (crank_direction) {
+            value |= FORWARD_DIRECTION;
         }
     }
-    if (!crank_direction) {
-        TCA9554_set(on? STARTER_FORWARD : STARTER_OFF);
-    } else {
-        TCA9554_set(on? STARTER_REVERSE : STARTER_OFF);
+
+    if (AP_HAL::millis() - fuel_prime_count < FUEL_PRIME_DURATION) {
+        value |= PMU_ARM;
     }
+
+    TCA9554_set(value);
 }
 
 #endif // AP_ICENGINE_TCA9554_STARTER_ENABLED
diff --git a/libraries/AP_ICEngine/AP_ICEngine_TCA9554.h b/libraries/AP_ICEngine/AP_ICEngine_TCA9554.h
@@ -9,23 +9,21 @@
 
 class AP_ICEngine_TCA9554 {
 public:
+    bool TCA9554_init();
     void set_starter(bool on, AP_Int8 crank_direction);
 
 private:
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_TCA9554;
 
-    enum TCA9554_state_t {
-        STARTER_OFF = 0x30, // output register - 0011 0000
-        STARTER_FORWARD = 0x11, // output register - 0001 0001 - Forward direction
-        STARTER_REVERSE = 0x01, // output register - 0000 0001 - Reverse direction
-    };
-    TCA9554_state_t last_state;
+    uint8_t prev_value;
 
     bool initialised;
 
-    bool TCA9554_init();
-    void TCA9554_set(TCA9554_state_t value);
+    // bool TCA9554_init();
+    void TCA9554_set(uint8_t value);
     uint32_t last_reg_check_ms;
+
+    uint32_t fuel_prime_count;
 };
 
 #endif // AP_ICENGINE_TCA9554_STARTER_ENABLED