Skip to content

Commit

Permalink
fix(el18): Inability to set custom failsafe (#3298)
Browse files Browse the repository at this point in the history 
* fix: Automatic power on and INRM301 failsafe BUG
  • Loading branch information
@Xy2019
Xy2019 authored Mar 22, 2023
1 parent f65f139 commit 366242e
Show file tree
Hide file tree
Showing 2 changed files with 58 additions and 29 deletions.
71 changes: 42 additions & 29 deletions radio/src/pulses/afhds3.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,8 @@
#define FAILSAFE_HOLD 1
#define FAILSAFE_CUSTOM 2

#define FAILSAFE_KEEP_LAST 0x8000

#define FAILSAFE_HOLD_VALUE 0x8000
#define FAILSAFE_NOPULSES_VALUE 0x8001
//get channel value outside of afhds3 namespace
int32_t getChannelValue(uint8_t channel);

Expand Down Expand Up @@ -225,7 +225,7 @@ class ProtoState

void requestInfoAndRun(bool send = false);

uint8_t setFailSafe(int16_t* target);
uint8_t setFailSafe(int16_t* target, uint8_t rfchannelcount=AFHDS3_MAX_CHANNELS);

inline int16_t convert(int channelValue);

Expand Down Expand Up @@ -276,7 +276,7 @@ class ProtoState
*/
ModuleVersion version;
};

static const char* const moduleStateText[] =
{
"Not ready",
Expand Down Expand Up @@ -399,7 +399,7 @@ void ProtoState::setupFrame()
if (this->state != ModuleState::STATE_STANDBY) {
auto mode = (uint8_t)MODULE_MODE_E::STANDBY;
trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &mode, 1);
return;
return;
} else {
modelIDSet = true;
modelID = g_model.header.modelId[module_index];
Expand All @@ -416,13 +416,13 @@ void ProtoState::setupFrame()
trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &mode, 1);
return;
}

if (this->state == ModuleState::STATE_STANDBY) {
cmdCount = 0;
requestInfoAndRun(true);
return;
}

// exit bind
if (this->state == STATE_BINDING) {
TRACE("AFHDS3 [EXIT BIND]");
Expand All @@ -441,16 +441,26 @@ void ProtoState::setupFrame()
COMMAND cmd = periodicRequestCommands[cmdIndex++];

if (cmd == COMMAND::VIRTUAL_FAILSAFE) {
if (isConnected() && !hasTelemetry()) {
TRACE("AFHDS ONE WAY FAILSAFE");
Config_u* cfg = this->getConfig();
uint8_t len =_phyMode_channels[cfg->v0.PhyMode];
if (!hasTelemetry()) {
uint16_t failSafe[AFHDS3_MAX_CHANNELS + 1] = {
((AFHDS3_MAX_CHANNELS << 8) | CHANNELS_DATA_MODE::FAIL_SAFE), 0};
setFailSafe((int16_t*)(&failSafe[1]));
((AFHDS3_MAX_CHANNELS << 8) | CHANNELS_DATA_MODE::FAIL_SAFE), 0};
setFailSafe((int16_t*)(&failSafe[1]), len);
TRACE("AFHDS ONE WAY FAILSAFE");
trsp.sendFrame(COMMAND::CHANNELS_FAILSAFE_DATA,
FRAME_TYPE::REQUEST_SET_NO_RESP, (uint8_t*)failSafe,
AFHDS3_MAX_CHANNELS * 2 + 2);
return;
}
else if( isConnected() ){
uint8_t data[AFHDS3_MAX_CHANNELS*2 + 3] = { (uint8_t)(RX_CMD_FAILSAFE_VALUE&0xFF), (uint8_t)((RX_CMD_FAILSAFE_VALUE>>8)&0xFF), (uint8_t)(2*len)};
int16_t failSafe[18];
setFailSafe(&failSafe[0], len);
std::memcpy( &data[3], failSafe, len );
trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, 2*len+3);
return;
}
} else {
trsp.sendFrame(cmd, FRAME_TYPE::REQUEST_GET_DATA);
return;
Expand Down Expand Up @@ -794,34 +804,37 @@ void ProtoState::applyConfigFromModel()

inline int16_t ProtoState::convert(int channelValue)
{
//pulseValue = limit<uint16_t>(0, 988 + ((channelValue + 1024) / 2), 0xfff);
//988 - 750 = 238
//238 * 20 = 4760
//2250 - 2012 = 238
//238 * 20 = 4760
// 988 ---- 2012
//-10240 ---- 10240
//-1024 ---- 1024
return ::limit<int16_t>(AFHDS3_FAILSAFE_MIN, channelValue * 10, AFHDS3_FAILSAFE_MAX);
return ::limit<int16_t>(AFHDS3_FAILSAFE_MIN, channelValue * 14.65, AFHDS3_FAILSAFE_MAX);
}

uint8_t ProtoState::setFailSafe(int16_t* target)
uint8_t ProtoState::setFailSafe(int16_t* target, uint8_t rfchannelsCount )
{
int16_t pulseValue = 0;
uint8_t channels_start = moduleData->channelsStart;
uint8_t channels_last = channels_start + 8 + moduleData->channelsCount;

for (uint8_t channel = channels_start; channel < channels_last; channel++) {
uint8_t channelsCount = 8 + moduleData->channelsCount;
uint8_t channels_last = channels_start + channelsCount;
std::memset(target, 0, 2*rfchannelsCount );
for (uint8_t channel = channels_start, i=0; i<rfchannelsCount && channel < channels_last; channel++, i++) {
if (moduleData->failsafeMode == FAILSAFE_CUSTOM) {
pulseValue = convert(g_model.failsafeChannels[channel]);
if(FAILSAFE_CHANNEL_HOLD==g_model.failsafeChannels[channel]){
pulseValue = FAILSAFE_HOLD_VALUE;
}else if(FAILSAFE_CHANNEL_NOPULSE==g_model.failsafeChannels[channel]){
pulseValue = FAILSAFE_NOPULSES_VALUE;
}
else{
pulseValue = convert(g_model.failsafeChannels[channel]);
}
}
else if (moduleData->failsafeMode == FAILSAFE_HOLD) {
pulseValue = FAILSAFE_KEEP_LAST;
pulseValue = FAILSAFE_HOLD_VALUE;
}
else if (moduleData->failsafeMode == FAILSAFE_NOPULSES) {
pulseValue = FAILSAFE_NOPULSES_VALUE;
}
else {
pulseValue = convert(::getChannelValue(channel));
pulseValue = FAILSAFE_NOPULSES_VALUE;
}
target[channel - channels_start] = pulseValue;
target[i] = pulseValue;
}
//return max channels because channel count can not be change after bind
return (uint8_t) (AFHDS3_MAX_CHANNELS);
Expand Down Expand Up @@ -871,7 +884,7 @@ static void* initModule(uint8_t module)
}

if (!mod_st) return nullptr;

auto p_state = &protoState[module];
p_state->init(module, pulsesGetModuleBuffer(module), mod_st, fAddr);
mod_st->user_data = (void*)p_state;
Expand Down
16 changes: 16 additions & 0 deletions radio/src/pulses/afhds3_transport.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,22 @@ enum COMMAND : uint8_t {
UNDEFINED = 0xFF
};

#define RX_CMD_RANGE ( 0x7013 )
#define RX_CMD_FAILSAFE_VALUE ( 0x6011 )
#define RX_CMD_FAILSAFE_TIME ( 0x6012 )
#define RX_CMD_RSSI_CHANNEL_SETUP ( 0x602B )
#define RX_CMD_GET_CAPABILITIES ( 0x7015 )
#define RX_CMD_OUT_PWM_PPM_MODE ( 0x7016 ) //PWM or PPM
#define RX_CMD_FREQUENCY_V0 ( 0x7017 )
#define RX_CMD_PORT_TYPE_V1 ( 0x7027 )
#define RX_CMD_FREQUENCY_V1 ( 0x7028 )
#define RX_CMD_FREQUENCY_V1_2 ( 0x7028 )
#define RX_CMD_BUS_TYPE_V0 ( 0x7018 ) //I-BUS/S-BUS
#define RX_CMD_IBUS_SETUP ( 0x7019 )
#define RX_CMD_IBUS_DIRECTION ( 0x7020 ) //IBUS INPUT or OUTPUT
#define RX_CMD_BUS_FAILSAFE ( 0x702A )
#define RX_CMD_GET_VERSION ( 0x701F )

// one byte frames for request queue
struct Frame {
enum COMMAND command;
Expand Down

0 comments on commit 366242e

Please sign in to comment.