From a68c66f5e4a4afcd869571fa5bb3c6e5f04e2c37 Mon Sep 17 00:00:00 2001 From: Pradeep CK Date: Tue, 10 Dec 2024 13:28:47 +1100 Subject: [PATCH] AP_ICEngine : Fuel pump implementation and cleanup --- libraries/AP_ICEngine/AP_ICEngine.cpp | 4 ++ libraries/AP_ICEngine/AP_ICEngine_TCA9554.cpp | 60 ++++++++++++------- libraries/AP_ICEngine/AP_ICEngine_TCA9554.h | 14 ++--- 3 files changed, 47 insertions(+), 31 deletions(-) diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index 7863037460..21740ac0df 100644 --- 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 index 3d5a9d1b59..072e82380e 100644 --- a/libraries/AP_ICEngine/AP_ICEngine_TCA9554.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine_TCA9554.cpp @@ -11,11 +11,11 @@ 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 @@ -23,18 +23,26 @@ extern const AP_HAL::HAL& hal; #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 index bb05257c7d..e584e8e5d4 100644 --- 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 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