AP_ICEngine: support relay for ignition and I2C for starter

libraries/AP_ICEngine/AP_ICEngine.cpp

@@ -24,6 +24,8 @@
 #include <AP_Notify/AP_Notify.h>
 #include <RC_Channel/RC_Channel.h>
 #include <AP_RPM/AP_RPM.h>
+#include <AP_Relay/AP_Relay.h>
+#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,29 +342,30 @@ 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;
         }
         starter_last_run_ms = now;
         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;

libraries/AP_ICEngine/AP_ICEngine.h

@@ -24,6 +24,13 @@
 
 #include <AP_Param/AP_Param.h>
 #include <Filter/LowPassFilter.h>
+#include <AP_RPM/AP_RPM_config.h>
+#include <AP_HAL/I2CDevice.h>
+#include <AP_Relay/AP_Relay.h>
+
+#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,13 +156,19 @@ 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 {
         bool flag;
         float governor_integrator;
         float throttle_percentage;
     } redline;
+#endif
 };
 
 

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

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<AP_HAL::I2CDevice> 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

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