Skip to content

Commit

Permalink
AP_ICEngine : Hirth engine - fix review comments
Browse files Browse the repository at this point in the history
  • Loading branch information
@Pradeep-Carbonix
Pradeep-Carbonix committed Nov 1, 2023
1 parent 28a53ac commit a15e479
Show file tree
Hide file tree
Showing 2 changed files with 161 additions and 3 deletions.
119 changes: 119 additions & 0 deletions libraries/AP_ICEngine/AP_ICEngine.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,8 @@
#include <RC_Channel/RC_Channel.h>
#include <AP_RPM/AP_RPM.h>
#include <AP_Parachute/AP_Parachute.h>
#include <AP_Relay/AP_Relay.h>
#include "AP_ICEngine.h"

extern const AP_HAL::HAL& hal;

Expand Down Expand Up @@ -163,9 +165,29 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = {
AP_GROUPINFO("REDLINE_RPM", 17, AP_ICEngine, redline_rpm, 0),
#endif

// @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
AP_GROUPINFO("IGNITION_RLY", 18, AP_ICEngine, ignition_relay, 0),

// @Param: CRNK_DIR
// @DisplayName: Crank Direction
// @Description: This is a flag to control which direction the PMU will crank Hirth Engine
// @User: Standard
// @Values: 0: Reverse, 1: Forward
AP_GROUPINFO("CRNK_DIR", 19, AP_ICEngine, crnk_dir_val, 1),

AP_GROUPEND
};

#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

// constructor
AP_ICEngine::AP_ICEngine()
Expand All @@ -192,6 +214,13 @@ void AP_ICEngine::update(void)
return;
}

if (i2c_state == I2C_UNINITIALIZED) {
i2c_state = I2C_FAILED;
if (TCA9554_init()) {
i2c_state = I2C_SUCCESS;
}
}

uint16_t cvalue = 1500;
RC_Channel *c = rc().channel(start_chan-1);
if (c != nullptr && rc().has_valid_input()) {
Expand Down Expand Up @@ -380,18 +409,36 @@ void AP_ICEngine::update(void)
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);
control_ign_str(IGN_OFF_STR_OFF);
ignition_relay_set(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);
control_ign_str(IGN_ON_STR_OFF);
ignition_relay_set(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);
if (!(hal.util->get_soft_armed() || allow_throttle_while_disarmed())) {
control_ign_str(IGN_ON_STR_OFF);
ignition_relay_set(false);
} else {
if (crnk_dir_val == true) { //CRNK_DIR is set to 1 (Forward)
control_ign_str(IGN_ON_STR_ON_DIR_ON_FWD);
ignition_relay_set(true);
} else { //CRNK_DIR is set to 0 (Reverse)
control_ign_str(IGN_ON_STR_ON_DIR_ON_REV);
ignition_relay_set(true);
}

}

if (starter_start_time_ms == 0) {
starter_start_time_ms = now;
}
Expand All @@ -401,6 +448,8 @@ void AP_ICEngine::update(void)
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);
control_ign_str(IGN_ON_STR_OFF);
ignition_relay_set(true);
starter_start_time_ms = 0;
break;
}
Expand Down Expand Up @@ -591,6 +640,76 @@ void AP_ICEngine::update_idle_governor(int8_t &min_throttle)
#endif // AP_RPM_ENABLED
}

bool AP_ICEngine::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());

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);
if (!ret) {
return false;
}
TCA9554_set(IGN_OFF_STR_OFF);
dev_TCA9554->set_retries(1);
return true;
}

void AP_ICEngine::TCA9554_set(TCA9554_state_t value)
{
if (value != TCA9554_state) {
TCA9554_state = value;
WITH_SEMAPHORE(dev_TCA9554->get_semaphore());
// set outputs and status leds
//dev_TCA9554->write_register(TCA9554_OUTPUT, (~(value<<2) & 0x0C) | value);
dev_TCA9554->write_register(TCA9554_OUTPUT, (~(value<<2) & 0x0C) | value);
//0011 0010
//1100 1000 & 0000 1100
//0000 1000 OR 0011 0010
//0011 1010
}
}

void AP_ICEngine::control_ign_str(TCA9554_state_t value)
{
if (i2c_state == I2C_SUCCESS)
{
TCA9554_set(value);
}
}

/*
control relay for ICE ignition
*/
void AP_ICEngine::ignition_relay_set(bool on)
{
if (ignition_relay > 0) {
auto *relay = AP::relay();
if (relay != nullptr) {
if (on) {
relay->on(ignition_relay-1);
} else {
relay->off(ignition_relay-1);
}
}
}
}

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;
namespace AP {
Expand Down
45 changes: 42 additions & 3 deletions libraries/AP_ICEngine/AP_ICEngine.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include <AP_Param/AP_Param.h>
#include <Filter/LowPassFilter.h>
#include <AP_RPM/AP_RPM_config.h>
#include <AP_HAL/I2CDevice.h>

class AP_ICEngine {
public:
Expand Down Expand Up @@ -58,15 +59,47 @@ class AP_ICEngine {
void update_idle_governor(int8_t &min_throttle);

// do we have throttle while disarmed enabled?
bool allow_throttle_while_disarmed(void) const {
return enable && option_set(Options::THROTTLE_WHILE_DISARMED);
}
bool allow_throttle_while_disarmed(void) const;

static AP_ICEngine *get_singleton() { return _singleton; }

private:
static AP_ICEngine *_singleton;

/*
* 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
*/

enum TCA9554_state_t {
IGN_OFF_STR_OFF = 0x30, //output register - 0011 0000
IGN_ON_STR_OFF = 0x30, //output register - 0011 0000
IGN_ON_STR_ON_DIR_ON_REV = 0x01, //output register - 0000 0001 - Reverse
IGN_ON_STR_ON_DIR_ON_FWD = 0x11, //output register - 0001 0001 - Forward
} TCA9554_state = IGN_OFF_STR_OFF;

enum i2c_init_t {
I2C_UNINITIALIZED = 0,
I2C_FAILED = 1,
I2C_SUCCESS = 2
} i2c_state = I2C_UNINITIALIZED;

bool TCA9554_init();

void TCA9554_set(TCA9554_state_t value);

void control_ign_str(TCA9554_state_t value);
void ignition_relay_set(bool on);

AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_TCA9554;

enum ICE_State state;

#if AP_RPM_ENABLED
Expand Down Expand Up @@ -128,6 +161,12 @@ class AP_ICEngine {
// Idle Controller Slew Rate
AP_Float idle_slew;
#endif

// relay number for ignition
AP_Int8 ignition_relay;

// Crank direction value
AP_Int8 crnk_dir_val;

// height when we enter ICE_START_HEIGHT_DELAY
float initial_height;
Expand Down

0 comments on commit a15e479

Please sign in to comment.