Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_ICEngine: support relay for ignition and I2C for starter

This commit is contained in:
Pradeep CK 2023-10-26 16:17:19 +11:00 committed by Andrew Tridgell
parent afd18cf13a
commit 07d76a87fd
5 changed files with 211 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
libraries/AP_ICEngine/AP_ICEngine.cpp
View File

@ -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;
@ -163,10 +165,18 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = {
AP_GROUPINFO("REDLINE_RPM", 17, AP_ICEngine, redline_rpm, 0),
#endif
#if AP_RELAY_ENABLED
// @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),
#endif
AP_GROUPEND
};
// constructor
AP_ICEngine::AP_ICEngine()
{
@ -378,20 +388,21 @@ void AP_ICEngine::update(void)
case ICE_DISABLED:
return;
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;
}
@ -399,8 +410,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;
}
@ -591,6 +602,42 @@ void AP_ICEngine::update_idle_governor(int8_t &min_throttle)
#endif // AP_RPM_ENABLED
}
/*
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);
#if AP_RELAY_ENABLED
// optionally use a relay as well
if (ignition_relay > 0) {
auto *relay = AP::relay();
if (relay != nullptr) {
relay->set(ignition_relay-1, on);
}
}
#endif // AP_RELAY_ENABLED
}
/*
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;
namespace AP {

24
libraries/AP_ICEngine/AP_ICEngine.h
View File

@ -25,6 +25,12 @@
#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_config.h>
#if AP_ICENGINE_TCA9554_STARTER_ENABLED
#include "AP_ICEngine_TCA9554.h"
#endif
class AP_ICEngine {
public:
@ -58,15 +64,16 @@ public:
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;
void set_ignition(bool on);
void set_starter(bool on);
enum ICE_State state;
#if AP_RPM_ENABLED
@ -128,7 +135,12 @@ private:
// Idle Controller Slew Rate
AP_Float idle_slew;
#endif
#if AP_RELAY_ENABLED
// relay number for ignition
AP_Int8 ignition_relay;
#endif
// height when we enter ICE_START_HEIGHT_DELAY
float initial_height;
@ -159,6 +171,10 @@ private:
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;

97
libraries/AP_ICEngine/AP_ICEngine_TCA9554.cpp Normal file
View File

@ -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

30
libraries/AP_ICEngine/AP_ICEngine_TCA9554.h Normal file
View File

@ -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

8
libraries/AP_ICEngine/AP_ICEngine_config.h
View File

@ -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