diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index 6e36b3c133..f5b2132866 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -24,6 +24,8 @@ #include #include #include +#include +#include "AP_ICEngine.h" extern const AP_HAL::HAL& hal; @@ -154,10 +156,16 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = { // @Units: RPM AP_GROUPINFO("REDLINE_RPM", 17, AP_ICEngine, redline_rpm, 0), + // @Param: IGNITION_RLY + // @DisplayName: Ignition relay channel + // @Description: This is a a relay channel to use for ignition control + // @User: Standard + // @Values: 0:None,1:Relay1,2:Relay2,3:Relay3,4:Relay4,5:Relay5,6:Relay6 + AP_GROUPINFO("IGNITION_RLY", 18, AP_ICEngine, ignition_relay, 0), + AP_GROUPEND }; - // constructor AP_ICEngine::AP_ICEngine(const AP_RPM &_rpm) : rpm(_rpm) @@ -334,20 +342,21 @@ void AP_ICEngine::update(void) /* now set output channels */ switch (state) { case ICE_OFF: - SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_off); - SRV_Channels::set_output_pwm(SRV_Channel::k_starter, pwm_starter_off); + set_ignition(false); + set_starter(false); starter_start_time_ms = 0; break; case ICE_START_HEIGHT_DELAY: case ICE_START_DELAY: - SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_on); - SRV_Channels::set_output_pwm(SRV_Channel::k_starter, pwm_starter_off); + set_ignition(true); + set_starter(false); break; case ICE_STARTING: - SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_on); - SRV_Channels::set_output_pwm(SRV_Channel::k_starter, pwm_starter_on); + set_ignition(true); + set_starter(true); + if (starter_start_time_ms == 0) { starter_start_time_ms = now; } @@ -355,8 +364,8 @@ void AP_ICEngine::update(void) break; case ICE_RUNNING: - SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_on); - SRV_Channels::set_output_pwm(SRV_Channel::k_starter, pwm_starter_off); + set_ignition(true); + set_starter(false); starter_start_time_ms = 0; break; } @@ -537,6 +546,39 @@ void AP_ICEngine::update_idle_governor(int8_t &min_throttle) min_throttle = roundf(idle_governor_integrator); } +/* + set ignition state + */ +void AP_ICEngine::set_ignition(bool on) +{ + SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, on? pwm_ignition_on : pwm_ignition_off); + // optionally use a relay as well + if (ignition_relay > 0) { + auto *relay = AP::relay(); + if (relay != nullptr) { + relay->set(ignition_relay-1, on); + } + } +} + +/* + set starter state + */ +void AP_ICEngine::set_starter(bool on) +{ + SRV_Channels::set_output_pwm(SRV_Channel::k_starter, on? pwm_starter_on : pwm_starter_off); + +#if AP_ICENGINE_TCA9554_STARTER_ENABLED + tca9554_starter.set_starter(on); +#endif +} + + +bool AP_ICEngine::allow_throttle_while_disarmed() const +{ + return option_set(Options::THROTTLE_WHILE_DISARMED) && + hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED; +} // singleton instance. Should only ever be set in the constructor. AP_ICEngine *AP_ICEngine::_singleton; diff --git a/libraries/AP_ICEngine/AP_ICEngine.h b/libraries/AP_ICEngine/AP_ICEngine.h index f5a1818f22..0833b91280 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.h +++ b/libraries/AP_ICEngine/AP_ICEngine.h @@ -24,6 +24,13 @@ #include #include +#include +#include +#include + +#if AP_ICENGINE_TCA9554_STARTER_ENABLED +#include "AP_ICEngine_TCA9554.h" +#endif class AP_ICEngine { public: @@ -62,6 +69,9 @@ class AP_ICEngine { const class AP_RPM &rpm; + void set_ignition(bool on); + void set_starter(bool on); + enum ICE_State state; // filter for RPM value @@ -115,7 +125,10 @@ class AP_ICEngine { // Idle Controller Slew Rate AP_Float idle_slew; - + + // relay number for ignition + AP_Int8 ignition_relay; + // height when we enter ICE_START_HEIGHT_DELAY float initial_height; @@ -143,6 +156,11 @@ class AP_ICEngine { uint16_t start_chan_last_value = 1500; uint32_t start_chan_last_ms; +#if AP_ICENGINE_TCA9554_STARTER_ENABLED + AP_ICEngine_TCA9554 tca9554_starter; +#endif + +#if AP_RPM_ENABLED // redline rpm AP_Int32 redline_rpm; struct { @@ -150,6 +168,7 @@ class AP_ICEngine { float governor_integrator; float throttle_percentage; } redline; +#endif }; diff --git a/libraries/AP_ICEngine/AP_ICEngine_TCA9554.cpp b/libraries/AP_ICEngine/AP_ICEngine_TCA9554.cpp new file mode 100644 index 0000000000..fddfa3363f --- /dev/null +++ b/libraries/AP_ICEngine/AP_ICEngine_TCA9554.cpp @@ -0,0 +1,97 @@ +#include "AP_ICEngine_config.h" + +/* + support for TCA9554 for starter control on I2C + */ + +#if AP_ICENGINE_TCA9554_STARTER_ENABLED +#include "AP_ICEngine.h" + +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) + * P2 = I2C_P2 - Unused + * P3 = LED (active low) + * P4 = PMU_START - Crank Direction (CN6 pin 5) + * 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_CONF 0x03 // Configuration Port register address [0 = Output] +#define TCA9554_PINS 0xC2 // Set all used ports to outputs = 1100 0010 + +/* + initialise TCA9554 + */ +bool AP_ICEngine_TCA9554::TCA9554_init() +{ + dev_TCA9554 = std::move(hal.i2c_mgr->get_device(TCA9554_I2C_BUS, TCA9554_I2C_ADDR)); + if (!dev_TCA9554) { + return false; + } + WITH_SEMAPHORE(dev_TCA9554->get_semaphore()); + + // setup 1 checked registers + dev_TCA9554->setup_checked_registers(1); + + dev_TCA9554->set_retries(10); + + // set outputs + bool ret = dev_TCA9554->write_register(TCA9554_OUTPUT, TCA9554_OUT_DEFAULT); + if (!ret) { + return false; + } + ret = dev_TCA9554->write_register(TCA9554_CONF, TCA9554_PINS, true); + if (!ret) { + return false; + } + + dev_TCA9554->set_retries(1); + return true; +} + +/* + set the state of the i2c controller + */ +void AP_ICEngine_TCA9554::TCA9554_set(TCA9554_state_t value) +{ + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_reg_check_ms > 100) { + /* + register checking at 10Hz allows us to cope with the i2c + device being power cycled after boot + */ + last_reg_check_ms = now_ms; + WITH_SEMAPHORE(dev_TCA9554->get_semaphore()); + dev_TCA9554->check_next_register(); + } + + if (value != last_state) { + 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; + } + } +} + +void AP_ICEngine_TCA9554::set_starter(bool on) +{ + if (!initialised) { + initialised = TCA9554_init(); + if (!initialised) { + // waiting for power to PMU + return; + } + } + TCA9554_set(on? STARTER_ON : STARTER_OFF); +} + +#endif // AP_ICENGINE_TCA9554_STARTER_ENABLED diff --git a/libraries/AP_ICEngine/AP_ICEngine_TCA9554.h b/libraries/AP_ICEngine/AP_ICEngine_TCA9554.h new file mode 100644 index 0000000000..005a5ece6c --- /dev/null +++ b/libraries/AP_ICEngine/AP_ICEngine_TCA9554.h @@ -0,0 +1,30 @@ +/* + optional control of starter via a TCA9554 I2C + */ + +#include "AP_ICEngine_config.h" + +#if AP_ICENGINE_TCA9554_STARTER_ENABLED +#include "AP_ICEngine.h" + +class AP_ICEngine_TCA9554 { +public: + void set_starter(bool on); + +private: + AP_HAL::OwnPtr dev_TCA9554; + + enum TCA9554_state_t { + STARTER_OFF = 0x30, // output register - 0011 0000 + STARTER_ON = 0x11, // output register - 0001 0001 - Forward direction + }; + TCA9554_state_t last_state; + + bool initialised; + + bool TCA9554_init(); + void TCA9554_set(TCA9554_state_t value); + uint32_t last_reg_check_ms; +}; + +#endif // AP_ICENGINE_TCA9554_STARTER_ENABLED diff --git a/libraries/AP_ICEngine/AP_ICEngine_config.h b/libraries/AP_ICEngine/AP_ICEngine_config.h index c609d429f1..24e7fd4770 100644 --- a/libraries/AP_ICEngine/AP_ICEngine_config.h +++ b/libraries/AP_ICEngine/AP_ICEngine_config.h @@ -5,3 +5,11 @@ #ifndef AP_ICENGINE_ENABLED #define AP_ICENGINE_ENABLED 1 #endif + +/* + optional TCA9554 I2C for starter control + */ +#ifndef AP_ICENGINE_TCA9554_STARTER_ENABLED +// enable on SITL by default to ensure code is built +#define AP_ICENGINE_TCA9554_STARTER_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif