gomaomao/DIY-Multiprotocol-TX-Module
1
0
Fork 0
mirror of https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git synced 2025-12-13 19:13:14 +00:00
Code Issues Projects Releases Wiki Activity

Added AFHDS2A BS4/BS6 support

Browse Source
This commit is contained in:
pascallanger
2025-03-20 14:49:34 +01:00
parent d091c57e10
commit db5f1f8899
8 changed files with 185 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
Multiprotocol/AFHDS2A_Rx_a7105.ino
View File

@@ -183,8 +183,18 @@ uint16_t AFHDS2A_RX_callback()
case AFHDS2A_RX_DATA:
if (AFHDS2A_RX_data_ready()) {
A7105_ReadData(AFHDS2A_RX_TXPACKET_SIZE);
if (memcmp(&packet[1], rx_id, 4) == 0 && memcmp(&packet[5], rx_tx_addr, 4) == 0) {
if (packet[0] == 0x58 && packet[37] == 0x00 && (telemetry_link&0x7F) == 0) { // standard packet, send channels to TX
if (memcmp(&packet[1], rx_id, 4) == 0 && memcmp(&packet[5], rx_tx_addr, 4) == 0)
{
#if 0
//if(packet[0] == 0xAA)
{
for(uint8_t i=0;i<AFHDS2A_RX_TXPACKET_SIZE;i++)
debug(" %02X",packet[i]);
debugln("");
}
#endif
if (packet[0] == 0x58 && packet[37] == 0x00 && (telemetry_link&0x7F) == 0)
{ // standard packet, send channels to TX
int rssi = min(A7105_ReadReg(A7105_1D_RSSI_THOLD),160);
RX_RSSI = map16b(rssi, 160, 8, 0, 128);
AFHDS2A_RX_build_telemetry_packet();

179
Multiprotocol/AFHDS2A_a7105.ino
View File

@@ -93,15 +93,17 @@ static void AFHDS2A_update_telemetry()
if (option & 0x80)
{// forward 0xAA and 0xAC telemetry to TX, skip rx and tx id to save space
packet_in[0]= TX_RSSI;
debug("T(%02X)=",packet[0]);
for(uint8_t i=9;i < AFHDS2A_RXPACKET_SIZE; i++)
{
packet_in[i-8]=packet[i];
debug(" %02X",packet[i]);
}
#if 0
debug("T(%02X)=",packet[0]);
for(uint8_t i=9;i < AFHDS2A_RXPACKET_SIZE; i++)
{
packet_in[i-8]=packet[i];
debug(" %02X",packet[i]);
}
debugln("");
#endif
packet_in[29]=packet[0]; // 0xAA Normal telemetry, 0xAC Extended telemetry
telemetry_link=2;
debugln("");
return;
}
#endif
@@ -186,16 +188,40 @@ static void AFHDS2A_build_packet(uint8_t type)
case AFHDS2A_PACKET_STICKS:
packet[0] = 0x58;
//16 channels + RX_LQI on channel 17
for(uint8_t ch=0; ch<num_ch; ch++)
for(uint8_t ch=0; ch<17; ch++)
{
if(ch == 16 // CH17=RX_LQI
#ifdef AFHDS2A_LQI_CH
|| ch == (AFHDS2A_LQI_CH-1) // override channel with LQI
#endif
)
val = 2000 - 10*RX_LQI;
val = convert_channel_ppm(sub_protocol<AFHDS2A_GYRO_OFF?CH_AETR[ch]:ch); // No remapping for BS receivers
if(ch<14)
{
packet[9 + ch*2] = val;
packet[10 + ch*2] = (val>>8)&0x0F;
}
else
val = convert_channel_ppm(CH_AETR[ch]);
{
if(ch == 16) //CH17=RX_LQI
val = 2000 - 10*RX_LQI;
packet[10 + (ch-14)*6] |= (val)<<4;
packet[12 + (ch-14)*6] |= (val)&0xF0;
packet[14 + (ch-14)*6] |= (val>>4)&0xF0;
}
}
{
uint8_t next_hop = (hopping_frequency_no+1)&0x0F;
packet[34] |= next_hop<<4;
packet[36] |= next_hop?0x80:0x90;
}
break;
case AFHDS2A_PACKET_FAILSAFE:
packet[0] = 0x56;
for(uint8_t ch=0; ch<16; ch++)
{ // Failsafe values
#ifdef FAILSAFE_ENABLE
val = Failsafe_data[protocol==PROTO_AFHDS2A?CH_AETR[ch]:ch]; // No remapping for BS receivers
if(val!=FAILSAFE_CHANNEL_HOLD && val!=FAILSAFE_CHANNEL_NOPULSES)
val = (((val<<2)+val)>>3)+860;
else
#endif
val = 0x0FFF;
if(ch<14)
{
packet[9 + ch*2] = val;
@@ -209,42 +235,6 @@ static void AFHDS2A_build_packet(uint8_t type)
}
}
break;
case AFHDS2A_PACKET_FAILSAFE:
packet[0] = 0x56;
for(uint8_t ch=0; ch<num_ch; ch++)
{
#ifdef FAILSAFE_ENABLE
if(ch<16)
val = Failsafe_data[CH_AETR[ch]];
else
val = FAILSAFE_CHANNEL_NOPULSES;
if(val!=FAILSAFE_CHANNEL_HOLD && val!=FAILSAFE_CHANNEL_NOPULSES)
{ // Failsafe values
val = (((val<<2)+val)>>3)+860;
if(ch<14)
{
packet[9 + ch*2] = val;
packet[10 + ch*2] = (val>>8)&0x0F;
}
else
{
packet[10 + (ch-14)*6] &= 0x0F;
packet[10 + (ch-14)*6] |= (val)<<4;
packet[12 + (ch-14)*6] &= 0x0F;
packet[12 + (ch-14)*6] |= (val)&0xF0;
packet[14 + (ch-14)*6] &= 0x0F;
packet[14 + (ch-14)*6] |= (val>>4)&0xF0;
}
}
else
#endif
if(ch<14)
{ // no values
packet[9 + ch*2] = 0xff;
packet[10+ ch*2] = 0xff;
}
}
break;
case AFHDS2A_PACKET_SETTINGS:
packet[0] = 0xaa;
packet[9] = 0xfd;
@@ -253,17 +243,43 @@ static void AFHDS2A_build_packet(uint8_t type)
if(val<50 || val>400) val=50; // default is 50Hz
packet[11]= val;
packet[12]= val >> 8;
packet[13] = sub_protocol & 0x01; // 1 -> PPM output enabled
packet[14]= 0x00;
for(uint8_t i=15; i<37; i++)
packet[i] = 0xff;
packet[18] = 0x05; // ?
packet[19] = 0xdc; // ?
packet[20] = 0x05; // ?
if(sub_protocol&2)
packet[21] = 0xdd; // SBUS output enabled
memset(&packet[15],0xFF,22);
#ifndef MULTI_AIR
if(sub_protocol < AFHDS2A_GYRO_OFF)
{
#endif
packet[13] = sub_protocol & 0x01; // 1 -> PPM output enabled
packet[14] = 0x00; // ?
packet[18] = 0x05; // ?
packet[19] = 0xdc; // ?
packet[20] = 0x05; // ?
if(sub_protocol&2)
packet[21] = 0xdd; // SBUS output enabled
else
packet[21] = 0xde; // IBUS
#ifndef MULTI_AIR
}
else
packet[21] = 0xde; // IBUS
{//BS receivers
if(sub_protocol == AFHDS2A_GYRO_OFF)
{
memset(&packet[15],0x00,4);
packet[22] = 0xFC; // ?
}
else
{//AFHDS2A_GYRO_ON & AFHDS2A_GYRO_ON_REV
packet[15] = convert_channel_16b_limit(CH13,0,100); // ST Gain
packet[16] = convert_channel_16b_limit(CH14,0,100); // TH Gain
packet[17] = convert_channel_16b_limit(CH15,0,100); // Priority
if(sub_protocol == AFHDS2A_GYRO_ON_REV)
packet[17] |= 0x80; // Reverse
packet[18] = CH16_SW?(0x32|0x80):0x32; // Calib|50?
packet[19] = 0x64; // 100?
packet[20] = 0x64; // 100?
packet[22] = 0xFE; // ?
}
}
#endif
break;
}
packet[37] = 0x00;
@@ -283,6 +299,9 @@ uint16_t AFHDS2A_callback()
static uint16_t packet_counter;
uint8_t data_rx=0;
uint16_t start;
#ifndef MULTI_AIR
static uint16_t Prev_Channel[4] = { 0,0,0,0 };
#endif
#ifndef FORCE_AFHDS2A_TUNING
A7105_AdjustLOBaseFreq(1);
#endif
@@ -367,10 +386,39 @@ uint16_t AFHDS2A_callback()
AFHDS2A_build_packet(packet_type);
data_rx=A7105_ReadReg(A7105_00_MODE); // Check if something has been received...
A7105_WriteData(AFHDS2A_TXPACKET_SIZE, hopping_frequency[hopping_frequency_no++]);
if(hopping_frequency_no >= AFHDS2A_NUMFREQ)
hopping_frequency_no = 0;
hopping_frequency_no &= 0x0F; // AFHDS2A_NUMFREQ
#if 0
for(uint8_t i=0; i<AFHDS2A_TXPACKET_SIZE; i++)
debug(" %02X",packet[i]);
debugln("");
#endif
#ifndef MULTI_AIR
if(sub_protocol > AFHDS2A_GYRO_OFF)
{//Gyro is on
//Check if gyro settings have changed
uint16_t val;
for(uint8_t i=0;i<4;i++)
{
val = Channel_data[CH13+i] - Prev_Channel[i];
if(val&0x8000) val ^= 0xFFFF;
if(val > 10)
{//This setting has significantly changed
Prev_Channel[i] = Channel_data[CH13+i];
packet_sent = 5;
}
}
}
if(packet_sent && (packet_counter%5)==0)
{//Inform the RX of the change
packet_type = AFHDS2A_PACKET_SETTINGS;
packet_sent--;
}
else
#endif
if(!(packet_counter % 1313))
{//Send settings every 5s
packet_type = AFHDS2A_PACKET_SETTINGS;
}
else
{
#ifdef FAILSAFE_ENABLE
@@ -448,9 +496,6 @@ void AFHDS2A_init()
rx_id[i]=eeprom_read_byte((EE_ADDR)(addr+i));
}
hopping_frequency_no = 0;
if(sub_protocol&0x04)
num_ch=17;
else
num_ch=14;
packet_sent = 0;
}
#endif

2
Multiprotocol/Multi.txt
View File

@@ -25,7 +25,7 @@
25,FrskyV
26,HONTAI,HONTAI,JJRCX1,X5C1,FQ777_951,XKK170
27,OpnLrs
28,AFHDS2A,PWM_IBUS,PPM_IBUS,PWM_SBUS,PPM_SBUS,PWM_IB16,PPM_IB16,PWM_SB16,PPM_SB16
28,AFHDS2A,PWM_IBUS,PPM_IBUS,PWM_SBUS,PPM_SBUS,Gyro_Off,Gyro_On,G_On_Rev
29,Q2X2,Q222,Q242,Q282
30,WK2x01,WK2801,WK2401,W6_5_1,W6_6_1,W6_HEL,W6_HEL_I
31,Q303,Q303,CX35,CX10D,CX10WD

4
Multiprotocol/Multi_Protos.ino
View File

@@ -147,7 +147,7 @@ const char STR_SUBTYPE_MT992[] = "\x04""PA18""SU35";
const char STR_SUBTYPE_MJXQ[] = "\x07""WLH08\0 ""X600\0 ""X800\0 ""H26D\0 ""E010\0 ""H26WH\0 ""Phoenix";
const char STR_SUBTYPE_FY326[] = "\x05""Std\0 ""FY319";
const char STR_SUBTYPE_HONTAI[] = "\x06""Std\0 ""JJRCX1""X5C1\0 ""FQ_951""XKK170";
const char STR_SUBTYPE_AFHDS2A[] = "\x08""PWM,IBUS""PPM,IBUS""PWM,SBUS""PPM,SBUS""PWM,IB16""PPM,IB16""PWM,SB16""PPM,SB16";
const char STR_SUBTYPE_AFHDS2A[] = "\x08""PWM,IBUS""PPM,IBUS""PWM,SBUS""PPM,SBUS""Gyro_Off""Gyro_On\0""G_On_Rev";
const char STR_SUBTYPE_Q2X2[] = "\x04""Q222""Q242""Q282";
const char STR_SUBTYPE_WK2x01[] = "\x06""WK2801""WK2401""W6_5_1""W6_6_1""W6_HeL""W6_HeI";
const char STR_SUBTYPE_Q303[] = "\x06""Std\0 ""CX35\0 ""CX10D\0""CX10WD";
@@ -307,7 +307,7 @@ const mm_protocol_definition multi_protocols[] = {
{PROTO_FLYSKY, STR_FLYSKY, STR_SUBTYPE_FLYSKY, 5, OPTION_NONE, 0, 1, SW_A7105, FLYSKY_init, FLYSKY_callback },
#endif
#if defined(AFHDS2A_A7105_INO)
{PROTO_AFHDS2A, STR_AFHDS2A, STR_SUBTYPE_AFHDS2A, 8, OPTION_SRVFREQ, 1, 1, SW_A7105, AFHDS2A_init, AFHDS2A_callback },
{PROTO_AFHDS2A, STR_AFHDS2A, STR_SUBTYPE_AFHDS2A, 7, OPTION_SRVFREQ, 1, 1, SW_A7105, AFHDS2A_init, AFHDS2A_callback },
#endif
#if defined(AFHDS2A_RX_A7105_INO)
{PROTO_AFHDS2A_RX, STR_AFHDS2A_RX,STR_CPPM, NBR_CPPM, OPTION_NONE, 0, 0, SW_A7105, AFHDS2A_RX_init, AFHDS2A_RX_callback },

17
Multiprotocol/Multiprotocol.h
View File

@@ -19,7 +19,7 @@
#define VERSION_MAJOR 1
#define VERSION_MINOR 3
#define VERSION_REVISION 4
#define VERSION_PATCH_LEVEL 46
#define VERSION_PATCH_LEVEL 47
#define MODE_SERIAL 0
@@ -159,14 +159,13 @@ enum Hubsan
};
enum AFHDS2A
{
PWM_IBUS = 0,
PPM_IBUS = 1,
PWM_SBUS = 2,
PPM_SBUS = 3,
PWM_IB16 = 4,
PPM_IB16 = 5,
PWM_SB16 = 6,
PPM_SB16 = 7,
PWM_IBUS = 0,
PPM_IBUS = 1,
PWM_SBUS = 2,
PPM_SBUS = 3,
AFHDS2A_GYRO_OFF = 4,
AFHDS2A_GYRO_ON = 5,
AFHDS2A_GYRO_ON_REV = 6,
};
enum Hisky
{

14
Multiprotocol/_Config.h
View File

@@ -211,7 +211,7 @@
#define FRSKYL_CC2500_INO
#define FRSKYD_CC2500_INO
#define FRSKYV_CC2500_INO
#define FRSKYX_CC2500_INO
#define FRSKYX_CC2500_INO //Include FRSKYX2 protocol
#define FRSKY_RX_CC2500_INO
#define HITEC_CC2500_INO
#define HOTT_CC2500_INO
@@ -302,11 +302,6 @@
//Enable DSM Forward Programming
#define DSM_FWD_PGM
//AFHDS2A specific settings
//-------------------------
//When enabled (remove the "//"), the below setting makes LQI (Link Quality Indicator) available on one of the RX ouput channel (5-14).
//#define AFHDS2A_LQI_CH 14
/**************************/
/*** FAILSAFE SETTINGS ***/
/**************************/
@@ -568,10 +563,9 @@ const PPM_Parameters PPM_prot[14*NBR_BANKS]= {
PPM_IBUS
PWM_SBUS
PPM_SBUS
PWM_IB16
PPM_IB16
PWM_SB16
PPM_SB16
AFHDS2A_GYRO_OFF
AFHDS2A_GYRO_ON
AFHDS2A_GYRO_ON_REV
PROTO_AFHDS2A_RX
NONE
PROTO_ASSAN