Skip to content

Commit

Permalink
fix(AFHDS3): iBus IN and Failsafe settings not updating (#3793)
Browse files Browse the repository at this point in the history
  • Loading branch information
@pfeerick
pfeerick committed Aug 4, 2023
1 parent 3f517e6 commit 7e29fc7
Show file tree
Hide file tree
Showing 2 changed files with 65 additions and 22 deletions.
73 changes: 57 additions & 16 deletions radio/src/pulses/afhds3.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -513,7 +513,6 @@ void ProtoState::setupFrame()
trsp.putFrame(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)};
Expand All @@ -524,8 +523,8 @@ void ProtoState::setupFrame()
}
} else {
trsp.putFrame(cmd, FRAME_TYPE::REQUEST_GET_DATA);
return;
}
return;
}

if (isConnected()) {
Expand Down Expand Up @@ -755,6 +754,7 @@ void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount)
} break;
case RX_CMD_IBUS_DIRECTION:
if(RX_CMDRESULT::RXSUCCESS==*data++) {
clearDirtyFlag(DC_RX_CMD_IBUS_DIRECTION);
this->cmd_flg &= ~0x10;
if( 0==cfg->version && 2==cfg->v0.ExternalBusType)
this->cmd_flg |= 0x20;
Expand Down Expand Up @@ -827,14 +827,14 @@ bool ProtoState::syncSettings()
}
else if( 2==receiver_type(rx_version.ProductNumber) )
{
if(this->cmd_flg&0x08)
if(this->cmd_flg&0x08)//IBUS
{
uint8_t data[] = { (uint8_t)(RX_CMD_BUS_TYPE_V0&0xFF), (uint8_t)((RX_CMD_BUS_TYPE_V0>>8)&0xFF), 0x1, 0 };
trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));
this->cmd_flg |= 0x10;
return true;
}
else if((this->cmd_flg&0x10))
else if((this->cmd_flg&0x10))//IBUS-IN IBUS-OUT
{
if(busdir>=2) busdir = 0; //IBUSOUT
uint8_t data1[] = { (uint8_t)(RX_CMD_IBUS_DIRECTION&0xFF), (uint8_t)((RX_CMD_IBUS_DIRECTION>>8)&0xFF), 0x1, busdir };
Expand All @@ -851,7 +851,7 @@ bool ProtoState::syncSettings()
}

// Sync settings when dirty flag is set
if (checkDirtyFlag(DC_RX_CMD_TX_PWR))
if (checkDirtyFlag(DC_RX_CMD_TX_PWR))
{
TRACE("AFHDS3 [RX_CMD_TX_PWR] %d", AFHDS3_POWER[moduleData->afhds3.rfPower] / 4);
uint8_t data[] = { (uint8_t)(RX_CMD_TX_PWR&0xFF), (uint8_t)((RX_CMD_TX_PWR>>8)&0xFF), 2,
Expand All @@ -860,7 +860,7 @@ bool ProtoState::syncSettings()
clearDirtyFlag(DC_RX_CMD_TX_PWR);
return true;
}
if (checkDirtyFlag(DC_RX_CMD_RSSI_CHANNEL_SETUP))
if (checkDirtyFlag(DC_RX_CMD_RSSI_CHANNEL_SETUP))
{
TRACE("AFHDS3 [RX_CMD_RSSI_CHANNEL_SETUP]");
uint8_t data[] = { (uint8_t)(RX_CMD_RSSI_CHANNEL_SETUP&0xFF), (uint8_t)((RX_CMD_RSSI_CHANNEL_SETUP>>8)&0xFF), 1, cfg->v1.SignalStrengthRCChannelNb };
Expand All @@ -874,49 +874,90 @@ bool ProtoState::syncSettings()
trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));
return true;
}
if (checkDirtyFlag(DC_RX_CMD_FREQUENCY_V0))
if (checkDirtyFlag(DC_RX_CMD_FREQUENCY_V0))
{
TRACE("AFHDS3 [RX_CMD_FREQUENCY_V0]");
uint16_t Frequency = ((cfg->v0.PWMFrequency.Synchronized<<15)| cfg->v0.PWMFrequency.Frequency);
uint8_t data[] = { (uint8_t)(RX_CMD_FREQUENCY_V0&0xFF), (uint8_t)((RX_CMD_FREQUENCY_V0>>8)&0xFF), 2,
(uint8_t)(cfg->v0.PWMFrequency.Frequency&0xFF), (uint8_t)((cfg->v0.PWMFrequency.Frequency>>8)&0xFF) };
(uint8_t)(Frequency&0xFF), (uint8_t)((Frequency>>8)&0xFF) };
trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));
return true;
}
if (checkDirtyFlag(DC_RX_CMD_PORT_TYPE_V1))
if (checkDirtyFlag(DC_RX_CMD_PORT_TYPE_V1))
{
TRACE("AFHDS3 [RX_CMD_PORT_TYPE_V1]");
uint8_t data[] = { (uint8_t)(RX_CMD_PORT_TYPE_V1&0xFF), (uint8_t)((RX_CMD_PORT_TYPE_V1>>8)&0xFF), 4, 0, 0, 0, 0 };
std::memcpy(&data[3], &cfg->v1.NewPortTypes, SES_NPT_NB_MAX_PORTS);
trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));
return true;
}
if (checkDirtyFlag(DC_RX_CMD_FREQUENCY_V1))
if (checkDirtyFlag(DC_RX_CMD_FREQUENCY_V1))
{
TRACE("AFHDS3 [RX_CMD_FREQUENCY_V1]");
uint8_t data[32 + 3 + 3] = { (uint8_t)(RX_CMD_FREQUENCY_V1&0xFF), (uint8_t)((RX_CMD_FREQUENCY_V1>>8)&0xFF), 32+3};
data[3] = 0;
std::memcpy(&data[4], &cfg->v1.PWMFrequenciesV1.PWMFrequencies[0], 32);
data[36] = cfg->v1.PWMFrequenciesV1.Synchronized & 0xff;
data[37] = (cfg->v1.PWMFrequenciesV1.Synchronized>>8) & 0xff;
data[37] = (cfg->v1.PWMFrequenciesV1.Synchronized>>8) & 0xff;
trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));
DIRTY_CMD(cfg, DC_RX_CMD_FREQUENCY_V1_2);
return true;
}
if (checkDirtyFlag(DC_RX_CMD_FREQUENCY_V1_2))
if (checkDirtyFlag(DC_RX_CMD_FREQUENCY_V1_2))
{
TRACE("AFHDS3 [RX_CMD_FREQUENCY_V1_2]");
uint8_t data[32 + 3 + 3] = { (uint8_t)(RX_CMD_FREQUENCY_V1_2&0xFF), (uint8_t)((RX_CMD_FREQUENCY_V1_2>>8)&0xFF), 32+3};
data[3] = 1;
std::memcpy(&data[4], &cfg->v1.PWMFrequenciesV1.PWMFrequencies[16], 32);
data[36] = (cfg->v1.PWMFrequenciesV1.Synchronized>>16) & 0xff;
data[37] = (cfg->v1.PWMFrequenciesV1.Synchronized>>24) & 0xff;
data[37] = (cfg->v1.PWMFrequenciesV1.Synchronized>>24) & 0xff;
trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));
return true;
}
if (checkDirtyFlag(DC_RX_CMD_BUS_TYPE_V0))
if (checkDirtyFlag(DC_RX_CMD_BUS_TYPE_V0))
{
TRACE("AFHDS3 [RX_CMD_BUS_TYPE_V0]");
uint8_t data[] = { (uint8_t)(RX_CMD_BUS_TYPE_V0&0xFF), (uint8_t)((RX_CMD_BUS_TYPE_V0>>8)&0xFF), 1, cfg->others.ExternalBusType };
bool onlySupportIBUSOut = (1==receiver_type(rx_version.ProductNumber));

if (onlySupportIBUSOut && cfg->others.ExternalBusType == EB_BT_IBUS1_IN)
{
cfg->others.ExternalBusType = EB_BT_IBUS1_OUT;
}
uint8_t data[] = { (uint8_t)(RX_CMD_BUS_TYPE_V0&0xFF), (uint8_t)((RX_CMD_BUS_TYPE_V0>>8)&0xFF), 1,
cfg->others.ExternalBusType == EB_BT_SBUS1 ? EB_BT_SBUS1 : EB_BT_IBUS1};
trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));

if (!onlySupportIBUSOut)
{
DIRTY_CMD(cfg, DC_RX_CMD_IBUS_DIRECTION);
}
return true;

/*
if(1>=cfg->others.ExternalBusType) //IBUS1
{
uint8_t data[] = { (uint8_t)(RX_CMD_BUS_TYPE_V0&0xFF), (uint8_t)((RX_CMD_BUS_TYPE_V0>>8)&0xFF), 1, 0 };
trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));
if( 1==receiver_type(rx_version.ProductNumber))
{
cfg->others.ExternalBusType = 0;//These RXs only support iBUS-OUT
}
cfg->others.iBusType = cfg->others.ExternalBusType;
DIRTY_CMD(cfg, DC_RX_CMD_IBUS_DIRECTION);
}
else if(2==cfg->others.ExternalBusType)//SBUS
{
uint8_t data[] = { (uint8_t)(RX_CMD_BUS_TYPE_V0&0xFF), (uint8_t)((RX_CMD_BUS_TYPE_V0>>8)&0xFF), 1, cfg->others.ExternalBusType };
trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));
}
return true;
*/
}
if (checkDirtyFlag(DC_RX_CMD_IBUS_DIRECTION))
{
TRACE("AFHDS3 [RX_CMD_IBUS_DIRECTION]");
uint8_t data[] = { (uint8_t)(RX_CMD_IBUS_DIRECTION&0xFF), (uint8_t)((RX_CMD_IBUS_DIRECTION>>8)&0xFF), 1,
cfg->others.ExternalBusType == EB_BT_IBUS1_OUT ? EB_BT_IBUS1_OUT : EB_BT_IBUS1_IN};
trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data));
return true;
}
Expand Down Expand Up @@ -1004,7 +1045,7 @@ void ProtoState::applyConfigFromModel()
cfg.v0.EMIStandard = moduleData->afhds3.emi;
cfg.v0.IsTwoWay = moduleData->afhds3.telemetry;
cfg.v0.PhyMode = moduleData->afhds3.phyMode;
cfg.v0.ExternalBusType = cfg.others.ExternalBusType==1?0:cfg.others.ExternalBusType;
cfg.v0.ExternalBusType = cfg.others.ExternalBusType==EB_BT_SBUS1 ? EB_BT_SBUS1 : EB_BT_IBUS1;
// Failsafe
setFailSafe(cfg.v0.FailSafe);
if (moduleData->failsafeMode != FAILSAFE_NOPULSES) {
Expand Down
14 changes: 8 additions & 6 deletions radio/src/pulses/afhds3_config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,17 +37,19 @@ enum eSES_PA_SetAnalogOutput {
SES_ANALOG_OUTPUT_PPM
};

enum eEB_BusType {
enum eEB_BusType : uint8_t {
EB_BT_IBUS1=0,
EB_BT_IBUS1_OUT = EB_BT_IBUS1,
EB_BT_IBUS2,
EB_BT_IBUS1_IN = EB_BT_IBUS2,
EB_BT_SBUS1
};

typedef enum
enum IBUS1_DIR
{
IBUS1_OUT,
IBUS1_IN, //Not yet supported
} eIBUS1_DIR;
IBUS1_IN,
};

// 48 bytes
PACK(struct sDATA_ConfigV0 {
Expand All @@ -61,7 +63,7 @@ PACK(struct sDATA_ConfigV0 {
uint8_t FailsafeOutputMode; //TRUE Or FALSE
sSES_PWMFrequencyV0 PWMFrequency;
uint8_t AnalogOutput; // eSES_PA_SetAnalogOutput
uint8_t ExternalBusType; // eEB_BusType
eEB_BusType ExternalBusType; // eEB_BusType
});

#define SES_NB_MAX_CHANNELS (32)
Expand Down Expand Up @@ -129,7 +131,7 @@ PACK(struct sDATA_ConfigV1 {

PACK(struct sDATA_Others {
uint8_t buffer[sizeof(sDATA_ConfigV1)];
uint8_t ExternalBusType; // eEB_BusType
uint8_t ExternalBusType; // eEB_BusType IBUS1:0;IBUS2:1(Not supported yet);SBUS:2
tmr10ms_t lastUpdated; // last updated time
bool isConnected; // specify if receiver is connected
uint32_t dirtyFlag; // mapped to commands that need to be issued to sync settings
Expand Down

0 comments on commit 7e29fc7

Please sign in to comment.