AFHDS2A protocol addition: UNTESTED

Protocol number: 28
Sub protocols:  PWM_IBUS = 0, PPM_IBUS = 1, PWM_SBUS = 2, PPM_SBUS = 3
Option value =0->50Hz, =1->400Hz, =2->5Hz
Extended channel range supported
Telemetry supported for voltage and RSSI (RX RSSI)
This commit is contained in:
pascallanger 2016-10-20 19:29:46 +02:00
parent a0186ce8e4
commit 999c630c5a
10 changed files with 411 additions and 36 deletions

View File

@ -31,13 +31,13 @@ void A7105_WriteData(uint8_t len, uint8_t channel)
A7105_Strobe(A7105_TX);
}
void A7105_ReadData()
void A7105_ReadData(uint8_t len)
{
uint8_t i;
A7105_Strobe(0xF0); //A7105_RST_RDPTR
A7105_CSN_off;
SPI_Write(0x45);
for (i=0;i<16;i++)
for (i=0;i<len;i++)
packet[i]=SPI_SDI_Read();
A7105_CSN_on;
}
@ -149,36 +149,61 @@ void A7105_Strobe(uint8_t address) {
SPI_Write(address);
A7105_CSN_on;
}
#ifdef HUBSAN_A7105_INO
const uint8_t PROGMEM HUBSAN_A7105_regs[] = {
0xFF, 0x63, 0xFF, 0x0F, 0xFF, 0xFF, 0xFF ,0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x05, 0x04, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x2B, 0xFF, 0xFF, 0x62, 0x80, 0xFF, 0xFF, 0x0A, 0xFF, 0xFF, 0x07,
0x17, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x47, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF
};
#endif
#ifdef FLYSKY_A7105_INO
const uint8_t PROGMEM FLYSKY_A7105_regs[] = {
0xff, 0x42, 0x00, 0x14, 0x00, 0xff, 0xff ,0x00, 0x00, 0x00, 0x00, 0x01, 0x21, 0x05, 0x00, 0x50,
0x9e, 0x4b, 0x00, 0x02, 0x16, 0x2b, 0x12, 0x00, 0x62, 0x80, 0x80, 0x00, 0x0a, 0x32, 0xc3, 0x0f,
0x13, 0xc3, 0x00, 0xff, 0x00, 0x00, 0x3b, 0x00, 0x17, 0x47, 0x80, 0x03, 0x01, 0x45, 0x18, 0x00,
0x01, 0x0f, 0xff
};
#endif
#ifdef AFHDS2A_A7105_INO
const uint8_t PROGMEM AFHDS2A_A7105_regs[] = {
0xFF, 0x42 | (1<<5), 0x00, 0x25, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x01, 0x3c, 0x05, 0x00, 0x50, // 00 - 0f
0x9e, 0x4b, 0x00, 0x02, 0x16, 0x2b, 0x12, 0x4f, 0x62, 0x80, 0xFF, 0xFF, 0x2a, 0x32, 0xc3, 0x1f, // 10 - 1f
0x1e, 0xFF, 0x00, 0xFF, 0x00, 0x00, 0x3b, 0x00, 0x17, 0x47, 0x80, 0x03, 0x01, 0x45, 0x18, 0x00, // 20 - 2f
0x01, 0x0f, 0xFF // 30 - 32
};
#endif
#define ID_NORMAL 0x55201041
#define ID_PLUS 0xAA201041
void A7105_Init(uint8_t protocol)
void A7105_Init(void)
{
uint8_t *A7105_Regs;
uint8_t *A7105_Regs=0;
if(protocol==INIT_FLYSKY)
{
A7105_WriteID(0x5475c52A);//0x2Ac57554
A7105_Regs=(uint8_t*)FLYSKY_A7105_regs;
}
else
#ifdef HUBSAN_A7105_INO
if(protocol==MODE_HUBSAN)
{
A7105_WriteID(ID_NORMAL);
A7105_Regs=(uint8_t*)HUBSAN_A7105_regs;
}
for (uint8_t i = 0; i < 0x33; i++){
else
#endif
{
A7105_WriteID(0x5475c52A);//0x2Ac57554
#ifdef FLYSKY_A7105_INO
if(protocol==MODE_FLYSKY)
A7105_Regs=(uint8_t*)FLYSKY_A7105_regs;
else
#endif
{
#ifdef AFHDS2A_A7105_INO
A7105_Regs=(uint8_t*)AFHDS2A_A7105_regs;
#endif
}
}
for (uint8_t i = 0; i < 0x33; i++)
{
if( pgm_read_byte_near(&A7105_Regs[i]) != 0xFF)
A7105_WriteReg(i, pgm_read_byte_near(&A7105_Regs[i]));
}
@ -190,7 +215,7 @@ void A7105_Init(uint8_t protocol)
// A7105_ReadReg(A7105_22_IF_CALIB_I);
// A7105_ReadReg(A7105_24_VCO_CURCAL);
if(protocol==INIT_FLYSKY)
if(protocol!=MODE_HUBSAN)
{
//VCO Current Calibration
A7105_WriteReg(A7105_24_VCO_CURCAL,0x13); //Recommended calibration from A7105 Datasheet
@ -211,8 +236,8 @@ void A7105_Init(uint8_t protocol)
// A7105_ReadReg(A7105_25_VCO_SBCAL_I);
//Reset VCO Band calibration
if(protocol==INIT_FLYSKY)
A7105_WriteReg(A7105_25_VCO_SBCAL_I,0x08);
if(protocol!=MODE_HUBSAN)
A7105_WriteReg(A7105_25_VCO_SBCAL_I,protocol==MODE_FLYSKY?0x08:0x0A);
A7105_SetTxRxMode(TX_EN);
A7105_SetPower();

View File

@ -0,0 +1,336 @@
/*
This project is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Multiprotocol is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
*/
// Last sync with hexfet new_protocols/flysky_a7105.c dated 2015-09-28
#ifdef AFHDS2A_A7105_INO
#define AFHDS2A_EEPROMadress 0 // rx ID 32bit
#define AFHDS2A_TXPACKET_SIZE 38
#define AFHDS2A_RXPACKET_SIZE 37
#define AFHDS2A_NUMFREQ 16
enum{
AFHDS2A_PACKET_STICKS,
AFHDS2A_PACKET_SETTINGS,
AFHDS2A_PACKET_FAILSAFE,
};
enum{
AFHDS2A_BIND1,
AFHDS2A_BIND2,
AFHDS2A_BIND3,
AFHDS2A_BIND4,
AFHDS2A_DATA,
};
static void AFHDS2A_calc_channels()
{
uint8_t idx = 0;
uint32_t rnd = MProtocol_id;
while (idx < AFHDS2A_NUMFREQ)
{
uint8_t i;
uint8_t count_1_42 = 0, count_43_85 = 0, count_86_128 = 0, count_129_168 = 0;
rnd = rnd * 0x0019660D + 0x3C6EF35F; // Randomization
uint8_t next_ch = ((rnd >> (idx%32)) % 0xa8) + 1;
// Keep the distance 2 between the channels - either odd or even
if (((next_ch ^ MProtocol_id) & 0x01 )== 0)
continue;
// Check that it's not duplicate and spread uniformly
for (i = 0; i < idx; i++)
{
if(hopping_frequency[i] == next_ch)
break;
if(hopping_frequency[i] <= 42)
count_1_42++;
else if (hopping_frequency[i] <= 85)
count_43_85++;
else if (hopping_frequency[i] <= 128)
count_86_128++;
else
count_129_168++;
}
if (i != idx)
continue;
if ((next_ch <= 42 && count_1_42 < 5)
||(next_ch >= 43 && next_ch <= 85 && count_43_85 < 5)
||(next_ch >= 86 && next_ch <=128 && count_86_128 < 5)
||(next_ch >= 129 && count_129_168 < 5))
hopping_frequency[idx++] = next_ch;
}
}
// telemetry sensors ID
enum{
AFHDS2A_SENSOR_RX_VOLTAGE = 0x00,
AFHDS2A_SENSOR_RX_ERR_RATE = 0xfe,
AFHDS2A_SENSOR_RX_RSSI = 0xfc,
AFHDS2A_SENSOR_RX_NOISE = 0xfb,
AFHDS2A_SENSOR_RX_SNR = 0xfa,
};
static void AFHDS2A_update_telemetry()
{
// AA | TXID | rx_id | sensor id | sensor # | value 16 bit big endian | sensor id ......
// max 7 sensors per packet
for(uint8_t sensor=0; sensor<7; sensor++)
{
uint8_t index = 9+(4*sensor);
switch(packet[index])
{
case AFHDS2A_SENSOR_RX_VOLTAGE:
v_lipo = packet[index+3]<<8 | packet[index+2];
telemetry_link=1;
break;
/*case AFHDS2A_SENSOR_RX_ERR_RATE:
// packet[index+2];
break;*/
case AFHDS2A_SENSOR_RX_RSSI:
RSSI_dBm = -packet[index+2];
break;
case 0xff:
return;
/*default:
// unknown sensor ID
break;*/
}
}
}
static void AFHDS2A_build_bind_packet()
{
uint8_t ch;
memcpy( &packet[1], rx_tx_addr, 4);
memset( &packet[5], 0xff, 4);
packet[10]= 0x00;
for(ch=0; ch<16; ch++)
packet[11+ch] = hopping_frequency[ch];
memset( &packet[27], 0xff, 10);
packet[37] = 0x00;
switch(phase)
{
case AFHDS2A_BIND1:
packet[0] = 0xbb;
packet[9] = 0x01;
break;
case AFHDS2A_BIND2:
case AFHDS2A_BIND3:
case AFHDS2A_BIND4:
packet[0] = 0xbc;
if(phase == AFHDS2A_BIND4)
{
memcpy( &packet[5], &rx_id, 4);
memset( &packet[11], 0xff, 16);
}
packet[9] = phase-1;
if(packet[9] > 0x02)
packet[9] = 0x02;
packet[27]= 0x01;
packet[28]= 0x80;
break;
}
}
static void AFHDS2A_build_packet(uint8_t type)
{
memcpy( &packet[1], rx_tx_addr, 4);
memcpy( &packet[5], rx_id, 4);
switch(type)
{
case AFHDS2A_PACKET_STICKS:
packet[0] = 0x58;
for(uint8_t ch=0; ch<14; ch++)
{
packet[9 + ch*2] = Servo_data[CH_AETR[ch]]&0xFF;
packet[10 + ch*2] = (Servo_data[CH_AETR[ch]]>>8)&0xFF;
}
packet[37] = 0x00;
break;
case AFHDS2A_PACKET_FAILSAFE:
packet[0] = 0x56;
for(uint8_t ch=0; ch<14; ch++)
{
/*if((Model.limits[ch].flags & CH_FAILSAFE_EN))
{
packet[9 + ch*2] = Servo_data[CH_AETR[ch]] & 0xff;
packet[10+ ch*2] = (Servo_data[CH_AETR[ch]] >> 8) & 0xff;
}
else*/
{
packet[9 + ch*2] = 0xff;
packet[10+ ch*2] = 0xff;
}
}
packet[37] = 0x00;
break;
case AFHDS2A_PACKET_SETTINGS:
packet[0] = 0xaa;
packet[9] = 0xfd;
packet[10]= 0xff;
packet[12]= 0x00;
if(option==0) //50Hz
packet[11]= 50;
else
if(option==2) //5Hz
packet[11]= 50;
else //400Hz
{
packet[11]= 400&0xFF;
packet[12]= 400>>8;
}
if(sub_protocol == PPM_IBUS || sub_protocol == PPM_SBUS)
packet[13] = 0x01; // PPM output enabled
else
packet[13] = 0x00;
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 == PWM_SBUS || sub_protocol == PPM_SBUS)
packet[21] = 0xdd; // SBUS output enabled
else
packet[21] = 0xde;
packet[37] = 0x00;
break;
}
}
#define AFHDS2A_WAIT_WRITE 0x80
uint16_t ReadAFHDS2A()
{
uint8_t packet_type = AFHDS2A_PACKET_STICKS;
switch(phase)
{
case AFHDS2A_BIND1:
case AFHDS2A_BIND2:
case AFHDS2A_BIND3:
A7105_Strobe(A7105_STANDBY);
AFHDS2A_build_bind_packet();
A7105_WriteData(AFHDS2A_TXPACKET_SIZE, packet_count%2 ? 0x0d : 0x8c);
if(A7105_ReadReg(A7105_00_MODE) == 0x1b)
{ // todo: replace with check crc+fec
A7105_Strobe(A7105_RST_RDPTR);
A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
if(packet[0] == 0xbc)
{
uint8_t temp=50+RX_num*4;
for(uint8_t i=0; i<4; i++)
{
rx_id[i] = packet[5+i];
eeprom_write_byte((EE_ADDR)(temp+i),rx_id[i]);
}
if(packet[9] == 0x01)
phase = AFHDS2A_BIND4;
}
}
packet_count++;
phase |= AFHDS2A_WAIT_WRITE;
return 1700;
case AFHDS2A_BIND1|AFHDS2A_WAIT_WRITE:
case AFHDS2A_BIND2|AFHDS2A_WAIT_WRITE:
case AFHDS2A_BIND3|AFHDS2A_WAIT_WRITE:
A7105_Strobe(A7105_RX);
phase &= ~AFHDS2A_WAIT_WRITE;
phase++;
if(phase > AFHDS2A_BIND3)
phase = AFHDS2A_BIND1;
return 2150;
case AFHDS2A_BIND4:
A7105_Strobe(A7105_STANDBY);
AFHDS2A_build_bind_packet();
A7105_WriteData(AFHDS2A_TXPACKET_SIZE, packet_count%2 ? 0x0d : 0x8c);
packet_count++;
bind_phase++;
if(bind_phase>=4)
{
packet_count=0;
hopping_frequency_no=1;
phase = AFHDS2A_DATA;
BIND_DONE;
}
phase |= AFHDS2A_WAIT_WRITE;
return 1700;
case AFHDS2A_BIND4|AFHDS2A_WAIT_WRITE:
A7105_Strobe(A7105_RX);
phase &= ~AFHDS2A_WAIT_WRITE;
return 2150;
case AFHDS2A_DATA:
A7105_Strobe(A7105_STANDBY);
AFHDS2A_build_packet(packet_type);
A7105_WriteData(AFHDS2A_TXPACKET_SIZE, hopping_frequency[hopping_frequency_no++]);
if(hopping_frequency_no >= 16)
hopping_frequency_no = 0;
if(!(packet_count % 1313))
packet_type = AFHDS2A_PACKET_SETTINGS;
else if(!(packet_count % 1569))
packet_type = AFHDS2A_PACKET_FAILSAFE;
else
packet_type = AFHDS2A_PACKET_STICKS; // todo : check for settings changes
// got some data from RX ?
// we've no way to know if RX fifo has been filled
// as we can't poll GIO1 or GIO2 to check WTR
// we can't check A7105_MASK_TREN either as we know
// it's currently in transmit mode.
if(!(A7105_ReadReg(A7105_00_MODE) & (1<<5 | 1<<6)))
{ // FECF+CRCF Ok
A7105_Strobe(A7105_RST_RDPTR);
A7105_ReadData(1);
if(packet[0] == 0xaa)
{
A7105_Strobe(A7105_RST_RDPTR);
A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
if(packet[9] == 0xfc)
packet_type=AFHDS2A_PACKET_SETTINGS; // RX is asking for settings
else
AFHDS2A_update_telemetry();
}
}
packet_count++;
phase |= AFHDS2A_WAIT_WRITE;
return 1700;
case AFHDS2A_DATA|AFHDS2A_WAIT_WRITE:
phase &= ~AFHDS2A_WAIT_WRITE;
A7105_Strobe(A7105_RX);
return 2150;
}
return 3850; // never reached, please the compiler
}
uint16_t initAFHDS2A()
{
A7105_Init();
AFHDS2A_calc_channels();
packet_count = 0;
bind_phase = 0;
if(IS_AUTOBIND_FLAG_on)
phase = AFHDS2A_BIND1;
else
{
phase = AFHDS2A_DATA;
//Read RX ID from EEPROM based on RX_num, RX_num must be uniq for each RX
uint8_t temp=50+RX_num*4;
for(uint8_t i=0;i<4;i++)
rx_id[i]=eeprom_read_byte((EE_ADDR)(temp+i));
}
hopping_frequency_no = 0;
return 50000;
}
#endif

View File

@ -184,7 +184,7 @@ uint16_t initFlySky()
uint8_t chanoffset;
uint8_t temp;
A7105_Init(INIT_FLYSKY); //flysky_init();
A7105_Init();
if ((rx_tx_addr[3]&0xF0) > 0x90) // limit offset to 9 as higher values don't work with some RX (ie V912)
rx_tx_addr[3]=rx_tx_addr[3]-0x70;

View File

@ -253,7 +253,7 @@ uint16_t ReadHubsan()
phase = BIND_1;
return 4500; //No signal, restart binding procedure. 12msec elapsed since last write
}
A7105_ReadData();
A7105_ReadData(16);
phase++;
if (phase == BIND_5)
A7105_WriteID(((uint32_t)packet[2] << 24) | ((uint32_t)packet[3] << 16) | ((uint32_t)packet[4] << 8) | packet[5]);
@ -264,7 +264,7 @@ uint16_t ReadHubsan()
phase = BIND_7;
return 15000; //22.5msec elapsed since last write
}
A7105_ReadData();
A7105_ReadData(16);
if(packet[1] == 9 && id_data == ID_NORMAL) {
phase = DATA_1;
A7105_WriteReg(A7105_1F_CODE_I, 0x0F);
@ -314,7 +314,7 @@ uint16_t ReadHubsan()
{
if( !(A7105_ReadReg(A7105_00_MODE) & 0x01))
{ // data received
A7105_ReadData();
A7105_ReadData(16);
if( hubsan_check_integrity() )
{
v_lipo=packet[13];// hubsan lipo voltage 8bits the real value is h_lipo/10(0x2A=42 -> 4.2V)
@ -343,7 +343,7 @@ uint16_t ReadHubsan()
uint16_t initHubsan() {
const uint8_t allowed_ch[] = {0x14, 0x1e, 0x28, 0x32, 0x3c, 0x46, 0x50, 0x5a, 0x64, 0x6e, 0x78, 0x82};
A7105_Init(INIT_HUBSAN); //hubsan_init();
A7105_Init();
sessionid = random(0xfefefefe) + ((uint32_t)random(0xfefefefe) << 16);
channel = allowed_ch[random(0xfefefefe) % sizeof(allowed_ch)];

View File

@ -46,6 +46,7 @@ enum PROTOCOLS
MODE_FRSKYV = 25, // =>CC2500
MODE_HONTAI = 26, // =>NRF24L01
MODE_OPENLRS = 27, // =>OpenLRS hardware
MODE_AFHDS2A = 28, // =>A7105
};
enum Flysky
@ -55,6 +56,13 @@ enum Flysky
V6X6 = 2,
V912 = 3
};
enum AFHDS2A
{
PWM_IBUS = 0,
PPM_IBUS = 1,
PWM_SBUS = 2,
PPM_SBUS = 3,
};
enum Hisky
{
Hisky = 0,
@ -378,6 +386,7 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p --
FrskyV 25
HONTAI 26
OpenLRS 27
AFHDS2A 28
BindBit=> 0x80 1=Bind/0=No
AutoBindBit=> 0x40 1=Yes /0=No
RangeCheck=> 0x20 1=Yes /0=No

View File

@ -66,6 +66,7 @@ uint16_t servo_max_100,servo_min_100,servo_max_125,servo_min_125;
// Protocol variables
uint8_t cyrfmfg_id[6];//for dsm2 and devo
uint8_t rx_tx_addr[5];
uint8_t rx_id[4];
uint8_t phase;
uint16_t bind_counter;
uint8_t bind_phase;
@ -93,7 +94,7 @@ uint8_t RX_num;
#endif
//Channel mapping for protocols
const uint8_t CH_AETR[]={AILERON, ELEVATOR, THROTTLE, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
const uint8_t CH_AETR[]={AILERON, ELEVATOR, THROTTLE, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8, AUX9, AUX10};
const uint8_t CH_TAER[]={THROTTLE, AILERON, ELEVATOR, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
const uint8_t CH_RETA[]={RUDDER, ELEVATOR, THROTTLE, AILERON, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
const uint8_t CH_EATR[]={ELEVATOR, AILERON, THROTTLE, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
@ -481,7 +482,7 @@ void Update_All()
#endif //ENABLE_PPM
update_led_status();
#if defined(TELEMETRY)
if((protocol==MODE_FRSKYD) || (protocol==MODE_HUBSAN) || (protocol==MODE_FRSKYX) || (protocol==MODE_DSM) )
if((protocol==MODE_FRSKYD) || (protocol==MODE_HUBSAN) || (protocol==MODE_AFHDS2A) || (protocol==MODE_FRSKYX) || (protocol==MODE_DSM) )
TelemetryUpdate();
#endif
}
@ -610,6 +611,13 @@ static void protocol_init()
remote_callback = ReadFlySky;
break;
#endif
#if defined(AFHDS2A_A7105_INO)
case MODE_AFHDS2A:
PE1_off; //antenna RF1
next_callback = initAFHDS2A();
remote_callback = ReadAFHDS2A;
break;
#endif
#if defined(HUBSAN_A7105_INO)
case MODE_HUBSAN:
PE1_off; //antenna RF1

View File

@ -210,4 +210,3 @@
#define AUX10 13
#define AUX11 14
#define AUX12 15
#define AUX13 16

View File

@ -130,7 +130,7 @@ void frsky_link_frame()
frame[4] = (uint8_t)RSSI_dBm;
}
else
if (protocol==MODE_HUBSAN)
if (protocol==MODE_HUBSAN||protocol==MODE_AFHDS2A)
{
frame[1] = v_lipo*2; //v_lipo; common 0x2A=42/10=4.2V
frame[2] = frame[1];
@ -486,7 +486,7 @@ void TelemetryUpdate()
}
#endif
if(telemetry_link && protocol != MODE_FRSKYX )
{ // FrSky + Hubsan
{ // FrSky + Hubsan + AFHDS2A
frsky_link_frame();
telemetry_link=0;
return;

View File

@ -57,6 +57,7 @@
//The protocols below need an A7105 to be installed
#define FLYSKY_A7105_INO
#define HUBSAN_A7105_INO
#define AFHDS2A_A7105_INO
//The protocols below need a CYRF6936 to be installed
#define DEVO_CYRF6936_INO
@ -255,6 +256,8 @@ const PPM_Parameters PPM_prot[15]= {
FORMAT_HONTAI
FORMAT_JJRCX1
FORMAT_X5C1
MODE_AFHDS2A
NONE
*/
// RX_Num is used for model match. Using RX_Num values different for each receiver will prevent starting a model with the false config loaded...

View File

@ -87,9 +87,4 @@ enum A7105_MASK {
A7105_MASK_VBCF = 1 << 3,
};
enum {
INIT_FLYSKY,
INIT_HUBSAN
};
#endif