mirror of
https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git
synced 2025-02-04 22:38:14 +00:00
362 lines
9.5 KiB
C++
362 lines
9.5 KiB
C++
/*
|
|
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
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
#if defined(AFHDS2A_FW_TELEMETRY) || defined(AFHDS2A_HUB_TELEMETRY)
|
|
// 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
|
|
#ifdef AFHDS2A_FW_TELEMETRY
|
|
if (option & 0x80)
|
|
{
|
|
// forward telemetry to TX, skip rx and tx id to save space
|
|
pkt[0]= TX_RSSI;
|
|
for(int i=9;i < AFHDS2A_RXPACKET_SIZE; i++)
|
|
pkt[i-8]=packet[i];
|
|
|
|
telemetry_link=2;
|
|
return;
|
|
}
|
|
#endif
|
|
#ifdef AFHDS2A_HUB_TELEMETRY
|
|
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_RX_ERR_RATE:
|
|
RX_LQI=packet[index+2];
|
|
break;
|
|
case AFHDS2A_SENSOR_RX_RSSI:
|
|
RX_RSSI = -packet[index+2];
|
|
break;
|
|
case 0xff:
|
|
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)
|
|
{
|
|
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;
|
|
}
|
|
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;
|
|
}
|
|
}
|
|
break;
|
|
case AFHDS2A_PACKET_SETTINGS:
|
|
packet[0] = 0xaa;
|
|
packet[9] = 0xfd;
|
|
packet[10]= 0xff;
|
|
uint16_t val_hz=5*(option & 0x7f)+50; // option value should be between 0 and 70 which gives a value between 50 and 400Hz
|
|
if(val_hz<50 || val_hz>400) val_hz=50; // default is 50Hz
|
|
packet[11]= val_hz;
|
|
packet[12]= val_hz >> 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; // IBUS
|
|
break;
|
|
}
|
|
packet[37] = 0x00;
|
|
}
|
|
|
|
#define AFHDS2A_WAIT_WRITE 0x80
|
|
uint16_t ReadAFHDS2A()
|
|
{
|
|
static uint8_t packet_type = AFHDS2A_PACKET_STICKS;
|
|
static uint16_t packet_counter=0;
|
|
uint8_t data_rx;
|
|
uint16_t start;
|
|
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 | 1<<6)))
|
|
{ // FECF+CRCF Ok
|
|
A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
|
|
if(packet[0] == 0xbc && packet[9] == 0x01)
|
|
{
|
|
uint8_t temp=50+RX_num*4;
|
|
uint8_t i;
|
|
for(i=0; i<4; i++)
|
|
{
|
|
rx_id[i] = packet[5+i];
|
|
eeprom_write_byte((EE_ADDR)(temp+i),rx_id[i]);
|
|
}
|
|
phase = AFHDS2A_BIND4;
|
|
packet_count++;
|
|
return 3850;
|
|
}
|
|
}
|
|
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:
|
|
//Wait for TX completion
|
|
start=micros();
|
|
while ((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 2150;
|
|
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)
|
|
{
|
|
packet_counter=0;
|
|
packet_type = AFHDS2A_PACKET_STICKS;
|
|
hopping_frequency_no=1;
|
|
phase = AFHDS2A_DATA;
|
|
BIND_DONE;
|
|
}
|
|
return 3850;
|
|
case AFHDS2A_DATA:
|
|
AFHDS2A_build_packet(packet_type);
|
|
if((A7105_ReadReg(A7105_00_MODE) & 0x01)) // Check if something has been received...
|
|
data_rx=0;
|
|
else
|
|
data_rx=1; // Yes
|
|
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 if(!(packet_counter % 1569))
|
|
packet_type = AFHDS2A_PACKET_FAILSAFE;
|
|
else
|
|
packet_type = AFHDS2A_PACKET_STICKS; // todo : check for settings changes
|
|
if(!(A7105_ReadReg(A7105_00_MODE) & (1<<5 | 1<<6)) && data_rx==1)
|
|
{ // RX+FECF+CRCF Ok
|
|
A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
|
|
if(packet[0] == 0xaa)
|
|
{
|
|
if(packet[9] == 0xfc)
|
|
packet_type=AFHDS2A_PACKET_SETTINGS; // RX is asking for settings
|
|
#if defined(AFHDS2A_FW_TELEMETRY) || defined(AFHDS2A_HUB_TELEMETRY)
|
|
else
|
|
{
|
|
// 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;
|
|
AFHDS2A_update_telemetry();
|
|
}
|
|
#endif
|
|
}
|
|
}
|
|
packet_counter++;
|
|
phase |= AFHDS2A_WAIT_WRITE;
|
|
return 1700;
|
|
case AFHDS2A_DATA|AFHDS2A_WAIT_WRITE:
|
|
//Wait for TX completion
|
|
start=micros();
|
|
while ((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 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;
|
|
#if defined(AFHDS2A_FW_TELEMETRY) || defined(AFHDS2A_HUB_TELEMETRY)
|
|
init_frskyd_link_telemetry();
|
|
#endif
|
|
return 50000;
|
|
}
|
|
#endif
|