Skip to content

Commit

Permalink
AP_ICEngine: Add Option to Control the direction of Cranking.
Browse files Browse the repository at this point in the history
Added the 5th bit to the ICE_OPTION parameter to enable CRANK_DIR_REVERSE control, specifically for managing Hirth engine direction using AP_ICEngine_TCA9554.
  • Loading branch information
loki077 authored and tridge committed Aug 19, 2024
1 parent 8e77f85 commit 94059ed
Show file tree
Hide file tree
Showing 4 changed files with 13 additions and 7 deletions.
4 changes: 2 additions & 2 deletions libraries/AP_ICEngine/AP_ICEngine.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,7 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = {
// @Param: OPTIONS
// @DisplayName: ICE options
// @Description: Options for ICE control. The Disable ignition in RC failsafe option will cause the ignition to be set off on any R/C failsafe. If Throttle while disarmed is set then throttle control will be allowed while disarmed for planes when in MANUAL mode. If disable while disarmed is set the engine will not start while the vehicle is disarmed unless overriden by the MAVLink DO_ENGINE_CONTROL command.
// @Bitmask: 0:Disable ignition in RC failsafe,1:Disable redline governor,2:Throttle while disarmed,3:Disable while disarmed
// @Bitmask: 0:Disable ignition in RC failsafe,1:Disable redline governor,2:Throttle while disarmed,3:Disable while disarmed,4:Crank direction Reverse
AP_GROUPINFO("OPTIONS", 15, AP_ICEngine, options, 0),

// @Param: STARTCHN_MIN
Expand Down Expand Up @@ -627,7 +627,7 @@ 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);
tca9554_starter.set_starter(on, option_set(Options::CRANK_DIR_REVERSE));
#endif

#if AP_RELAY_ENABLED
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_ICEngine/AP_ICEngine.h
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,7 @@ class AP_ICEngine {
DISABLE_REDLINE_GOVERNOR = (1U << 1),
THROTTLE_WHILE_DISARMED = (1U << 2),
NO_RUNNING_WHILE_DISARMED = (1U << 3),
CRANK_DIR_REVERSE = (1U << 4),
};
AP_Int16 options;

Expand Down
8 changes: 6 additions & 2 deletions libraries/AP_ICEngine/AP_ICEngine_TCA9554.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@ void AP_ICEngine_TCA9554::TCA9554_set(TCA9554_state_t value)
}
}

void AP_ICEngine_TCA9554::set_starter(bool on)
void AP_ICEngine_TCA9554::set_starter(bool on, bool crank_dir_reverse)
{
if (!initialised) {
initialised = TCA9554_init();
Expand All @@ -91,7 +91,11 @@ void AP_ICEngine_TCA9554::set_starter(bool on)
return;
}
}
TCA9554_set(on? STARTER_ON : STARTER_OFF);
if (!crank_dir_reverse) {
TCA9554_set(on? STARTER_FORWARD : STARTER_OFF);
} else {
TCA9554_set(on? STARTER_REVERSE : STARTER_OFF);
}
}

#endif // AP_ICENGINE_TCA9554_STARTER_ENABLED
7 changes: 4 additions & 3 deletions libraries/AP_ICEngine/AP_ICEngine_TCA9554.h
Original file line number Diff line number Diff line change
Expand Up @@ -9,14 +9,15 @@

class AP_ICEngine_TCA9554 {
public:
void set_starter(bool on);
void set_starter(bool on, bool crank_dir_reverse);

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

Expand Down

0 comments on commit 94059ed

Please sign in to comment.