gomaomao/DIY-Multiprotocol-TX-Module
1
0
Fork 0
mirror of https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git synced 2025-02-04 21:08:12 +00:00
Code Issues Projects Releases Wiki Activity
DIY-Multiprotocol-TX-Module/Multiprotocol/AFHDS2A_a7105.ino
Pascal Langer 49c3af12f9 AFHDS2A: fix issue with fs-x6b
2021-02-14 20:51:15 +01:00

450 lines
12 KiB
C++
Raw Blame History

/*
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_TXPACKET_SIZE 38
#define AFHDS2A_RXPACKET_SIZE 37
#define AFHDS2A_NUMFREQ 16
#if not defined TELEMETRY
uint8_t RX_LQI=0;
#endif
enum{
AFHDS2A_PACKET_STICKS,
AFHDS2A_PACKET_SETTINGS,
AFHDS2A_PACKET_FAILSAFE,
};
enum{
AFHDS2A_BIND1,
AFHDS2A_BIND2,
AFHDS2A_BIND3,
AFHDS2A_BIND4,
AFHDS2A_DATA_INIT,
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 band_no = ((((idx<<1) | ((idx>>1) & 0b01)) + rx_tx_addr[3]) & 0b11);
rnd = rnd * 0x0019660D + 0x3C6EF35F; // Randomization
uint8_t next_ch = band_no*41 + 1 + ((rnd >> idx) % 41); // Channel range: 1..164
for (i = 0; i < idx; i++)
{
// Keep the distance 5 between the channels
uint8_t distance;
if (next_ch > hopping_frequency[i])
distance = next_ch - hopping_frequency[i];
else
distance = hopping_frequency[i] - next_ch;
if (distance < 5) break;
}
if (i != idx) continue;
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,
AFHDS2A_SENSOR_A3_VOLTAGE = 0x03,
};
#if defined(AFHDS2A_FW_TELEMETRY) || defined(AFHDS2A_HUB_TELEMETRY)
static void AFHDS2A_update_telemetry()
{
// Read TX RSSI
int16_t temp=256-(A7105_ReadReg(A7105_1D_RSSI_THOLD)*8)/5; // value from A7105 is between 8 for maximum signal strength to 160 or less
if(temp<0) temp=0;
else if(temp>255) temp=255;
TX_RSSI=temp;
// AA | TXID | rx_id | sensor id | sensor # | value 16 bit big endian | sensor id ......
// AC | TXID | rx_id | sensor id | sensor # | length | bytes | sensor id ......
#ifdef AFHDS2A_FW_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]);
}
packet_in[29]=packet[0]; // 0xAA Normal telemetry, 0xAC Extended telemetry
telemetry_link=2;
debugln("");
return;
}
#endif
#ifdef AFHDS2A_HUB_TELEMETRY
if(packet[0]==0xAA)
{ // 0xAA Normal telemetry, 0xAC Extended telemetry not decoded here
for(uint8_t sensor=0; sensor<7; sensor++)
{
// Send FrSkyD telemetry to TX
uint8_t index = 9+(4*sensor);
switch(packet[index])
{
case AFHDS2A_SENSOR_RX_VOLTAGE:
//v_lipo1 = packet[index+3]<<8 | packet[index+2];
v_lipo1 = packet[index+2];
telemetry_link=1;
break;
case AFHDS2A_SENSOR_A3_VOLTAGE:
v_lipo2 = (packet[index+3]<<5) | (packet[index+2]>>3); // allows to read voltage up to 4S
telemetry_link=1;
break;
case AFHDS2A_SENSOR_RX_ERR_RATE:
if(packet[index+2]<=100)
RX_LQI=packet[index+2];
break;
case AFHDS2A_SENSOR_RX_RSSI:
RX_RSSI = -packet[index+2];
break;
case 0xff: // end of data
return;
/*default:
// unknown sensor ID
break;*/
}
}
}
#endif
}
#endif
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<AFHDS2A_NUMFREQ; 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)
{
uint16_t val;
memcpy( &packet[1], rx_tx_addr, 4);
memcpy( &packet[5], rx_id, 4);
switch(type)
{
case AFHDS2A_PACKET_STICKS:
packet[0] = 0x58;
//16 channels + RX_LQI on channel 17
for(uint8_t ch=0; ch<num_ch; 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;
else
val = convert_channel_ppm(CH_AETR[ch]);
if(ch<14)
{
packet[9 + ch*2] = val;
packet[10 + ch*2] = (val>>8)&0x0F;
}
else
{
packet[10 + (ch-14)*6] |= (val)<<4;
packet[12 + (ch-14)*6] |= (val)&0xF0;
packet[14 + (ch-14)*6] |= (val>>4)&0xF0;
}
}
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;
packet[10]= 0xff;
val=5*(option & 0x7f)+50; // option value should be between 0 and 70 which gives a value between 50 and 400Hz
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
else
packet[21] = 0xde; // IBUS
break;
}
packet[37] = 0x00;
}
#define AFHDS2A_WAIT_WRITE 0x80
#ifdef STM32_BOARD
#define AFHDS2A_WRITE_TIME 1550
#else
#define AFHDS2A_WRITE_TIME 1700
#endif
uint16_t AFHDS2A_callback()
{
static uint8_t packet_type;
static uint16_t packet_counter;
uint8_t data_rx=0;
uint16_t start;
#ifndef FORCE_AFHDS2A_TUNING
A7105_AdjustLOBaseFreq(1);
#endif
switch(phase)
{
case AFHDS2A_BIND1:
case AFHDS2A_BIND2:
case AFHDS2A_BIND3:
AFHDS2A_build_bind_packet();
A7105_WriteData(AFHDS2A_TXPACKET_SIZE, packet_count%2 ? 0x0d : 0x8c);
if(!(A7105_ReadReg(A7105_00_MODE) & (1<<5))) // removed FECF check due to issues with fs-x6b -> & (1<<5 | 1<<6)
{ // CRCF Ok
A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
if(packet[0] == 0xbc && packet[9] == 0x01)
{
uint16_t addr;
if(RX_num<16)
addr=AFHDS2A_EEPROM_OFFSET+RX_num*4;
else
addr=AFHDS2A_EEPROM_OFFSET2+(RX_num-16)*4;
for(uint8_t i=0; i<4; i++)
{
rx_id[i] = packet[5+i];
eeprom_write_byte((EE_ADDR)(addr+i),rx_id[i]);
}
phase = AFHDS2A_BIND4;
packet_count++;
break;
}
}
packet_count++;
if(IS_BIND_DONE)
{ // exit bind if asked to do so from the GUI
phase = AFHDS2A_BIND4;
break;
}
phase |= AFHDS2A_WAIT_WRITE;
return AFHDS2A_WRITE_TIME;
case AFHDS2A_BIND1|AFHDS2A_WAIT_WRITE:
case AFHDS2A_BIND2|AFHDS2A_WAIT_WRITE:
case AFHDS2A_BIND3|AFHDS2A_WAIT_WRITE:
//Wait for TX completion
start=micros();
while ((uint16_t)((uint16_t)micros()-start) < 700) // Wait max 700µs, using serial+telemetry exit in about 120µs
if(!(A7105_ReadReg(A7105_00_MODE) & 0x01))
break;
A7105_SetPower();
A7105_SetTxRxMode(TXRX_OFF); // Turn LNA off since we are in near range and we want to prevent swamping
A7105_Strobe(A7105_RX);
phase &= ~AFHDS2A_WAIT_WRITE;
phase++;
if(phase > AFHDS2A_BIND3)
phase = AFHDS2A_BIND1;
return 3850-AFHDS2A_WRITE_TIME;
case AFHDS2A_BIND4:
AFHDS2A_build_bind_packet();
A7105_WriteData(AFHDS2A_TXPACKET_SIZE, packet_count%2 ? 0x0d : 0x8c);
packet_count++;
bind_phase++;
if(bind_phase>=4)
{
hopping_frequency_no=1;
phase = AFHDS2A_DATA_INIT;
BIND_DONE;
}
break;
case AFHDS2A_DATA_INIT:
packet_counter=0;
packet_type = AFHDS2A_PACKET_STICKS;
phase = AFHDS2A_DATA;
case AFHDS2A_DATA:
#ifdef MULTI_SYNC
telemetry_set_input_sync(3850);
#endif
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;
if(!(packet_counter % 1313))
packet_type = AFHDS2A_PACKET_SETTINGS;
else
{
#ifdef FAILSAFE_ENABLE
if(!(packet_counter % 1569) && IS_FAILSAFE_VALUES_on)
{
packet_type = AFHDS2A_PACKET_FAILSAFE;
FAILSAFE_VALUES_off;
}
else
#endif
packet_type = AFHDS2A_PACKET_STICKS; // todo : check for settings changes
}
if(!(A7105_ReadReg(A7105_00_MODE) & (1<<5)) && !(data_rx & 1)) // removed FECF check due to issues with fs-x6b -> & (1<<5 | 1<<6)
{ // RX+CRCF Ok
A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
if(packet[0] == 0xAA && packet[9] == 0xFC)
packet_type=AFHDS2A_PACKET_SETTINGS; // RX is asking for settings
else
if((packet[0] == 0xAA && packet[9]!=0xFD) || packet[0] == 0xAC)
{// Normal telemetry packet, ignore packets which contain the RX configuration: AA FD FF 32 00 01 00 FF FF FF 05 DC 05 DE FA FF FF FF FF FF FF FF FF FF FF FF FF FF FF
if(!memcmp(&packet[1], rx_tx_addr, 4))
{ // TX address validated
for(uint8_t sensor=0; sensor<7; sensor++)
{//read LQI value for RX output
uint8_t index = 9+(4*sensor);
if(packet[index]==AFHDS2A_SENSOR_RX_ERR_RATE && packet[index+2]<=100)
{
RX_LQI=packet[index+2];
break;
}
}
#if defined(AFHDS2A_FW_TELEMETRY) || defined(AFHDS2A_HUB_TELEMETRY)
AFHDS2A_update_telemetry();
#endif
}
}
}
packet_counter++;
phase |= AFHDS2A_WAIT_WRITE;
return AFHDS2A_WRITE_TIME;
case AFHDS2A_DATA|AFHDS2A_WAIT_WRITE:
//Wait for TX completion
start=micros();
while ((uint16_t)((uint16_t)micros()-start) < 700) // Wait max 700µs, using serial+telemetry exit in about 120µs
if(!(A7105_ReadReg(A7105_00_MODE) & 0x01))
break;
A7105_SetPower();
A7105_SetTxRxMode(RX_EN);
A7105_Strobe(A7105_RX);
phase &= ~AFHDS2A_WAIT_WRITE;
return 3850-AFHDS2A_WRITE_TIME;
}
return 3850;
}
void AFHDS2A_init()
{
A7105_Init();
AFHDS2A_calc_channels();
packet_count = 0;
bind_phase = 0;
if(IS_BIND_IN_PROGRESS)
phase = AFHDS2A_BIND1;
else
{
phase = AFHDS2A_DATA_INIT;
//Read RX ID from EEPROM based on RX_num, RX_num must be uniq for each RX
uint16_t addr;
if(RX_num<16)
addr=AFHDS2A_EEPROM_OFFSET+RX_num*4;
else
addr=AFHDS2A_EEPROM_OFFSET2+(RX_num-16)*4;
for(uint8_t i=0;i<4;i++)
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;
}
#endif
Reference in New Issue View Git Blame Copy Permalink