DIY-Multiprotocol-TX-Module/Multiprotocol/Flysky_afhds2a_a7105.ino

// adaptation de https://github.com/goebish/deviation/blob/c5dd9fcc1441fc05fe9effa4c378886aeb3938d4/src/protocol/flysky_afhds2a_a7105.c
#ifdef AFHDS2A_A7105_INO
#define EEPROMadress	0		// rx ID 32bit
#define SERVO_HZ		50		//Frequency's servo 0=50	1=400	2=5		

#define TXPACKET_SIZE	38
#define RXPACKET_SIZE	37
#define NUMFREQ			16

static uint8_t txid[4];
static uint8_t rxid[4];
static uint8_t packet_type;
static uint8_t bind_reply;


enum{
	PACKET_STICKS,
	PACKET_SETTINGS,
	PACKET_FAILSAFE,
};

enum{
	BIND1,
	BIND2,
	BIND3,
	BIND4,
	DATA1,
};
enum {
	PWM_IBUS = 0,
	PPM_IBUS,
	PWM_SBUS,
	PPM_SBUS
};


static void build_packet(uint8_t type) {
	switch(type) {
		case PACKET_STICKS:		
			packet[0] = 0x58;
			memcpy( &packet[1], txid, 4);
			memcpy( &packet[5], rxid, 4);
			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 PACKET_FAILSAFE:
			packet[0] = 0x56;
			memcpy( &packet[1], txid, 4);
			memcpy( &packet[5], rxid, 4);
			for(uint8_t ch=0; ch<14; ch++) {
				if(ch==0) {
	//			if(ch < Model.num_channels && (Model.limits[ch].flags & CH_FAILSAFE_EN)) {
					packet[9 + ch*2] = Servo_data[AUX11] & 0xff;
					packet[10+ ch*2] = (Servo_data[AUX11] >> 8) & 0xff;
				}
				else {
					packet[9 + ch*2] = 0xff;
					packet[10+ ch*2] = 0xff;
				}
			}
			packet[37] = 0x00;
			break;
			
		case PACKET_SETTINGS:
			packet[0] = 0xaa;
			memcpy( &packet[1], txid, 4);
			memcpy( &packet[5], rxid, 4);
			packet[9] = 0xfd;
			packet[10]= 0xff;
			packet[11]= SERVO_HZ & 0xff;
			packet[12]= (SERVO_HZ >> 8) & 0xff;
			if(option == PPM_IBUS || option == 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(option == PWM_SBUS || option == PPM_SBUS)
				packet[21] = 0xdd; // SBUS output enabled
			else
				packet[21] = 0xde;
			packet[37] = 0x00;
			break;
	}
}

// telemetry sensors ID
enum{
	SENSOR_RX_VOLTAGE   = 0x00,
	SENSOR_RX_ERR_RATE  = 0xfe,
	SENSOR_RX_RSSI      = 0xfc,
	SENSOR_RX_NOISE     = 0xfb,
	SENSOR_RX_SNR       = 0xfa,
};

static void update_telemetry() {
	// AA | TXID | RXID | 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 SENSOR_RX_VOLTAGE:
				v_lipo = packet[index+3]<<8 | packet[index+2];
				telemetry_link=1;
				break;
			case SENSOR_RX_ERR_RATE:
				// packet[index+2];
				break;
			case 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], txid, 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 BIND1:
			packet[0] = 0xbb;
			packet[9] = 0x01;
			break;
		case BIND2:
		case BIND3:
		case BIND4:
			packet[0] = 0xbc;
			if(phase == BIND4) {
				memcpy( &packet[5], &rxid, 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 calc_afhds_channels() {
	int idx = 0;
	uint32_t rnd = MProtocol_id;
	while (idx < NUMFREQ) {
		int i;
		int 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;
		}
	}
}

#define WAIT_WRITE 0x80
static uint16_t afhds2a_cb() {
	switch(phase) {
		case BIND1:
		case BIND2:
		case BIND3:    
			A7105_Strobe(A7105_STANDBY);
			afhds2a_build_bind_packet();
			A7105_WriteData(38, packet_count%2 ? 0x0d : 0x8c);
			if(A7105_ReadReg(0) == 0x1b) { // todo: replace with check crc+fec
				A7105_Strobe(A7105_RST_RDPTR);
				A7105_ReadData(RXPACKET_SIZE);
				if(packet[0] == 0xbc) {
					for(uint8_t i=0; i<4; i++) {
						rxid[i] = packet[5+i];
					}
					eeprom_write_block((const void*)rxid,(EE_ADDR)EEPROMadress,4);
					if(packet[9] == 0x01)
						phase = BIND4;
				}
			}
			packet_count++;
			phase |= WAIT_WRITE;
			return 1700;
		case BIND1|WAIT_WRITE:
		case BIND2|WAIT_WRITE:
		case BIND3|WAIT_WRITE:
			A7105_Strobe(A7105_RX);
			phase &= ~WAIT_WRITE;
			phase++;
			if(phase > BIND3)
				phase = BIND1;
			return 2150;
		case BIND4:
			A7105_Strobe(A7105_STANDBY);
			afhds2a_build_bind_packet();
			A7105_WriteData(38, packet_count%2 ? 0x0d : 0x8c);
			packet_count++;
			bind_reply++;
			if(bind_reply>=4) { 
				packet_count=0;
				channel=1;
				phase = DATA1;
				BIND_DONE;
			}                        
			phase |= WAIT_WRITE;
			return 1700;
		case BIND4|WAIT_WRITE:
			A7105_Strobe(A7105_RX);
			phase &= ~WAIT_WRITE;
			return 2150;
		case DATA1:    
			A7105_Strobe(A7105_STANDBY);
			build_packet(packet_type);
			A7105_WriteData(38, hopping_frequency[channel++]);
			if(channel >= 16)
				channel = 0;
			if(!(packet_count % 1313))
				packet_type = PACKET_SETTINGS;
			else if(!(packet_count % 1569))
				packet_type = PACKET_FAILSAFE;
			else
				packet_type = 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(0) & (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(RXPACKET_SIZE);
					if(packet[9] == 0xfc) { // rx is asking for settings
						packet_type=PACKET_SETTINGS;
					}
					else {
						update_telemetry();
					}
				}
			}
			packet_count++;
			phase |= WAIT_WRITE;
			return 1700;
		case DATA1|WAIT_WRITE:
			phase &= ~WAIT_WRITE;
			A7105_Strobe(A7105_RX);
			return 2150;
	}
	return 3850; // never reached, please the compiler
}

static uint16_t AFHDS2A_setup()
{
	A7105_Init(INIT_FLYSKY_AFHDS2A);	//flysky_init();
	for (uint8_t i = 0; i < 4; ++i) {
		txid[i] = rx_tx_addr[i];
	}

	calc_afhds_channels();
	packet_type = PACKET_STICKS;
	packet_count = 0;
	bind_reply = 0;
	if(IS_AUTOBIND_FLAG_on) {
		phase = BIND1;
		BIND_IN_PROGRESS;
	}
	else {
		phase = DATA1;
		eeprom_read_block((void*)rxid,(EE_ADDR)EEPROMadress,4);
	}
	channel = 0;
	return 50000;
}
#endif