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

AFHDS2A extended telemetry forward

Browse Source
This commit is contained in:
Pascal Langer 2019-05-10 18:17:09 +02:00
parent 1916eb3095
commit 1a8f9c5a12
2 changed files with 53 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
Multiprotocol/AFHDS2A_a7105.ino
View File

@ -87,42 +87,49 @@ static void AFHDS2A_update_telemetry()
// max 7 sensors per packet // max 7 sensors per packet
#ifdef AFHDS2A_FW_TELEMETRY #ifdef AFHDS2A_FW_TELEMETRY
if (option & 0x80) if (option & 0x80)
{ {// forward 0xAA and 0xAC telemetry to TX, skip rx and tx id to save space
// forward telemetry to TX, skip rx and tx id to save space
pkt[0]= TX_RSSI; pkt[0]= TX_RSSI;
debug("T=");
for(int i=9;i < AFHDS2A_RXPACKET_SIZE; i++) for(int i=9;i < AFHDS2A_RXPACKET_SIZE; i++)
{
pkt[i-8]=packet[i]; pkt[i-8]=packet[i];
debug(" %02X",packet[i]);
}
telemetry_link=2; telemetry_link=2;
debugln("");
return; return;
} }
#endif #endif
#ifdef AFHDS2A_HUB_TELEMETRY #ifdef AFHDS2A_HUB_TELEMETRY
for(uint8_t sensor=0; sensor<7; sensor++) if(packet[0]==0xAA)
{ { // 0xAA Normal telemetry, 0xAC Extended telemetry not decoded here
// Send FrSkyD telemetry to TX for(uint8_t sensor=0; sensor<7; sensor++)
uint8_t index = 9+(4*sensor);
switch(packet[index])
{ {
case AFHDS2A_SENSOR_RX_VOLTAGE: // Send FrSkyD telemetry to TX
//v_lipo1 = packet[index+3]<<8 | packet[index+2]; uint8_t index = 9+(4*sensor);
v_lipo1 = packet[index+2]; switch(packet[index])
telemetry_link=1; {
break; case AFHDS2A_SENSOR_RX_VOLTAGE:
case AFHDS2A_SENSOR_A3_VOLTAGE: //v_lipo1 = packet[index+3]<<8 | packet[index+2];
v_lipo2 = (packet[index+3]<<5) | (packet[index+2]>>3); // allows to read voltage up to 4S v_lipo1 = packet[index+2];
telemetry_link=1; telemetry_link=1;
break; break;
case AFHDS2A_SENSOR_RX_ERR_RATE: case AFHDS2A_SENSOR_A3_VOLTAGE:
RX_LQI=packet[index+2]; v_lipo2 = (packet[index+3]<<5) | (packet[index+2]>>3); // allows to read voltage up to 4S
break; telemetry_link=1;
case AFHDS2A_SENSOR_RX_RSSI: break;
RX_RSSI = -packet[index+2]; case AFHDS2A_SENSOR_RX_ERR_RATE:
break; RX_LQI=packet[index+2];
case 0xff: break;
return; case AFHDS2A_SENSOR_RX_RSSI:
/*default: RX_RSSI = -packet[index+2];
// unknown sensor ID break;
break;*/ case 0xff: // end of data
return;
/*default:
// unknown sensor ID
break;*/
}
} }
} }
#endif #endif
@ -323,33 +330,26 @@ uint16_t ReadAFHDS2A()
if(!(A7105_ReadReg(A7105_00_MODE) & (1<<5 | 1<<6)) && data_rx==1) if(!(A7105_ReadReg(A7105_00_MODE) & (1<<5 | 1<<6)) && data_rx==1)
{ // RX+FECF+CRCF Ok { // RX+FECF+CRCF Ok
A7105_ReadData(AFHDS2A_RXPACKET_SIZE); A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
#ifdef DEBUG_SERIAL if(packet[0] == 0xAA && packet[9] == 0xFC)
debug("T="); packet_type=AFHDS2A_PACKET_SETTINGS; // RX is asking for settings
for(uint8_t i=0;i < AFHDS2A_RXPACKET_SIZE; i++) else
if(packet[0] == 0xAA || packet[0] == 0xAC)
{ {
debug(" %02X",packet[i]); if(memcmp(&packet[1], rx_tx_addr, 4))
{ // Validate TX address
#ifdef AFHDS2A_LQI_CH
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)
RX_LQI=packet[index+2];
}
#endif
#if defined(AFHDS2A_FW_TELEMETRY) || defined(AFHDS2A_HUB_TELEMETRY)
AFHDS2A_update_telemetry();
#endif
}
} }
debugln("");
#endif
if(packet[0] == 0xaa)
{
if(packet[9] == 0xfc)
packet_type=AFHDS2A_PACKET_SETTINGS; // RX is asking for settings
else
{
#ifdef AFHDS2A_LQI_CH
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)
RX_LQI=packet[index+2];
}
#endif
#if defined(AFHDS2A_FW_TELEMETRY) || defined(AFHDS2A_HUB_TELEMETRY)
AFHDS2A_update_telemetry();
#endif
}
}
} }
packet_counter++; packet_counter++;
phase |= AFHDS2A_WAIT_WRITE; phase |= AFHDS2A_WAIT_WRITE;

2
Multiprotocol/Multiprotocol.h
View File

@ -19,7 +19,7 @@
#define VERSION_MAJOR 1 #define VERSION_MAJOR 1
#define VERSION_MINOR 2 #define VERSION_MINOR 2
#define VERSION_REVISION 1 #define VERSION_REVISION 1
#define VERSION_PATCH_LEVEL 43 #define VERSION_PATCH_LEVEL 44
//****************** //******************
// Protocols // Protocols