gomaomao/DIY-Multiprotocol-TX-Module
1
0
Fork 0
mirror of https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git synced 2025-02-04 17:48:11 +00:00
Code Issues Projects Releases Wiki Activity
DIY-Multiprotocol-TX-Module/Multiprotocol/Telemetry.ino
Pascal Langer dbc33951a4 Fix for module boot issue?
2021-03-13 18:18:07 +01:00

1397 lines
34 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/>.
*/
//**************************
// Telemetry serial code *
//**************************
#if defined TELEMETRY
uint8_t RetrySequence ;
#ifdef MULTI_TELEMETRY
uint32_t lastMulti = 0;
#define MULTI_TIME 500 //in ms
#ifdef MULTI_SYNC
#define INPUT_SYNC_TIME 100 //in ms
#define INPUT_ADDITIONAL_DELAY 100 // in 10µs, 100 => 1000 µs
uint32_t lastInputSync = 0;
uint16_t inputDelay = 0;
#endif // MULTI_SYNC
#endif // MULTI_TELEMETRY
#if defined SPORT_TELEMETRY
#define FRSKY_SPORT_PACKET_SIZE 8
#define FX_BUFFERS 4
uint8_t Sport_Data = 0;
uint8_t pktx1[FRSKY_SPORT_PACKET_SIZE*FX_BUFFERS];
// Store for out of sequence packet
uint8_t FrSkyX_RX_ValidSeq ;
struct t_FrSkyX_RX_Frame
{
boolean valid;
uint8_t count;
uint8_t payload[6];
} ;
// Store for FrskyX telemetry
struct t_FrSkyX_RX_Frame FrSkyX_RX_Frames[4] ;
uint8_t FrSkyX_RX_NextFrame=0;
#endif // SPORT_TELEMETRY
#if defined HUB_TELEMETRY
#define USER_MAX_BYTES 6
uint8_t prev_index;
struct t_FrSkyD_User_Frame
{
uint8_t ID;
uint8_t low;
uint8_t high;
} FrSkyD_User_Frame[8];
uint8_t FrSkyD_User_Frame_Start=0, FrSkyD_User_Frame_End=0;
#endif // HUB_TELEMETRY
#define START_STOP 0x7e
#define BYTESTUFF 0x7d
#define STUFF_MASK 0x20
#define MAX_PKTX 10
uint8_t pktx[MAX_PKTX];
uint8_t frame[18];
#ifdef MULTI_TELEMETRY
static void multi_send_header(uint8_t type, uint8_t len)
{
Serial_write('M');
Serial_write('P');
Serial_write(type);
Serial_write(len);
}
#ifdef MULTI_SYNC
static void telemetry_set_input_sync(uint16_t refreshRate)
{
#if defined(STM32_BOARD) && defined(DEBUG_PIN)
static uint8_t c=0;
if (c++%2==0)
{ DEBUG_PIN_on; }
else
{ DEBUG_PIN_off; }
#endif
// Only record input Delay after a frame has really been received
// Otherwise protocols with faster refresh rates then the TX sends (e.g. 3ms vs 6ms) will screw up the calcualtion
inputRefreshRate = refreshRate;
if (last_serial_input != 0)
{
cli(); // Disable global int due to RW of 16 bits registers
inputDelay = TCNT1;
sei(); // Enable global int
//inputDelay = (inputDelay - last_serial_input)>>1;
inputDelay -= last_serial_input;
//if(inputDelay & 0x8000)
// inputDelay = inputDelay - 0x8000;
last_serial_input=0;
}
}
#endif
#ifdef MULTI_SYNC
static void mult_send_inputsync()
{
multi_send_header(MULTI_TELEMETRY_SYNC, 6);
Serial_write(inputRefreshRate >> 8);
Serial_write(inputRefreshRate & 0xff);
// Serial_write(inputDelay >> 8);
// Serial_write(inputDelay & 0xff);
Serial_write(inputDelay >> 9);
Serial_write(inputDelay >> 1);
Serial_write(INPUT_SYNC_TIME);
Serial_write(INPUT_ADDITIONAL_DELAY);
}
#endif //MULTI_SYNC
static void multi_send_status()
{
multi_send_header(MULTI_TELEMETRY_STATUS, 24);
// Build flags
uint8_t flags=0;
if (IS_INPUT_SIGNAL_on)
flags |= 0x01;
if (mode_select==MODE_SERIAL)
flags |= 0x02;
if (remote_callback != 0)
{
flags |= 0x04;
if(multi_protocols_index == 0xFF)
{
if(protocol!=PROTO_SCANNER)
flags &= ~0x04; //Invalid protocol
}
else if(sub_protocol&0x07)
{
uint8_t nbr=multi_protocols[multi_protocols_index].nbrSubProto;
//if(protocol==PROTO_DSM) nbr++; //Auto sub_protocol
if((sub_protocol&0x07)>=nbr)
flags &= ~0x04; //Invalid sub protocol
}
if (IS_WAIT_BIND_on)
flags |= 0x10;
else
if (IS_BIND_IN_PROGRESS)
flags |= 0x08;
if(multi_protocols_index != 0xFF)
{
if(multi_protocols[multi_protocols_index].chMap)
flags |= 0x40; //Disable_ch_mapping supported
#ifdef FAILSAFE_ENABLE
if(multi_protocols[multi_protocols_index].failSafe)
flags |= 0x20; //Failsafe supported
#endif
}
if(IS_DATA_BUFFER_LOW_on)
flags |= 0x80;
}
Serial_write(flags);
// Version number example: 1.1.6.1
Serial_write(VERSION_MAJOR);
Serial_write(VERSION_MINOR);
Serial_write(VERSION_REVISION);
Serial_write(VERSION_PATCH_LEVEL);
// Channel order
Serial_write(RUDDER<<6|THROTTLE<<4|ELEVATOR<<2|AILERON);
if(multi_protocols_index == 0xFF) // selection out of list... send first available protocol
{
Serial_write(multi_protocols[0].protocol); // begining of list
Serial_write(multi_protocols[0].protocol); // begining of list
for(uint8_t i=0;i<16;i++)
Serial_write(0x00); // everything else is invalid
}
else
{
// Protocol next/prev
if(multi_protocols[multi_protocols_index+1].protocol != 0)
{
if(multi_protocols[multi_protocols_index+1].protocol == PROTO_SCANNER)
{// if next is scanner
if(multi_protocols[multi_protocols_index+2].protocol != 0)
Serial_write(multi_protocols[multi_protocols_index+2].protocol); // skip to next protocol number
else
Serial_write(multi_protocols[multi_protocols_index].protocol); // or end of list
}
else
Serial_write(multi_protocols[multi_protocols_index+1].protocol); // next protocol number
}
else
Serial_write(multi_protocols[multi_protocols_index].protocol); // end of list
if(multi_protocols_index>0)
{
if(multi_protocols[multi_protocols_index-1].protocol==PROTO_SCANNER)
{// if prev is scanner
if(multi_protocols_index > 1)
Serial_write(multi_protocols[multi_protocols_index-2].protocol); // skip to prev protocol number
else
Serial_write(multi_protocols[multi_protocols_index].protocol); // begining of list
}
else
Serial_write(multi_protocols[multi_protocols_index-1].protocol); // prev protocol number
}
else
Serial_write(multi_protocols[multi_protocols_index].protocol); // begining of list
// Protocol
for(uint8_t i=0;i<7;i++)
Serial_write(multi_protocols[multi_protocols_index].ProtoString[i]); // protocol name
// Sub-protocol
uint8_t nbr=multi_protocols[multi_protocols_index].nbrSubProto;
Serial_write(nbr | (multi_protocols[multi_protocols_index].optionType<<4)); // number of sub protocols && option type
uint8_t j=0;
if(nbr && (sub_protocol&0x07)<nbr)
{
uint8_t len=multi_protocols[multi_protocols_index].SubProtoString[0];
uint8_t offset=len*(sub_protocol&0x07)+1;
for(;j<len;j++)
Serial_write(multi_protocols[multi_protocols_index].SubProtoString[j+offset]); // current sub protocol name
}
for(;j<8;j++)
Serial_write(0x00);
}
}
#ifdef MULTI_CONFIG_INO
void CONFIG_frame()
{
multi_send_header(MULTI_TELEMETRY_CONFIG, packet_in[0]);
for (uint8_t i = 1; i <= packet_in[0]; i++) // config data
Serial_write(packet_in[i]);
}
#endif
#ifdef MLINK_FW_TELEMETRY
void MLINK_frame()
{
multi_send_header(MULTI_TELEMETRY_MLINK, 10);
Serial_write(TX_RSSI); // RSSI
Serial_write(TX_LQI); // LQI
for (uint8_t i = 0; i < 8; i++) // followed by 8 bytes of telemetry data
Serial_write(packet_in[i]);
}
#endif
#ifdef DSM_TELEMETRY
void DSM_frame()
{
if (packet_in[0] == 0x80)
{
multi_send_header(MULTI_TELEMETRY_DSMBIND, 10);
for (uint8_t i = 1; i < 11; i++) // 10 bytes of DSM bind response
Serial_write(packet_in[i]);
}
else
{
multi_send_header(MULTI_TELEMETRY_DSM, 17);
for (uint8_t i = 0; i < 17; i++) // RSSI value followed by 16 bytes of telemetry data
Serial_write(packet_in[i]);
}
}
#endif
#ifdef SCANNER_TELEMETRY
void spectrum_scanner_frame()
{
multi_send_header(MULTI_TELEMETRY_SCANNER, SCAN_CHANS_PER_PACKET + 1);
Serial_write(packet_in[0]); // start channel
for(uint8_t ch = 0; ch < SCAN_CHANS_PER_PACKET; ch++)
Serial_write(packet_in[ch+1]); // RSSI power levels
}
#endif
#if defined (FRSKY_RX_TELEMETRY) || defined (AFHDS2A_RX_TELEMETRY) || defined (BAYANG_RX_TELEMETRY) || defined (DSM_RX_CYRF6936_INO)
void receiver_channels_frame()
{
uint16_t len = packet_in[3] * 11; // 11 bit per channel
if (len % 8 == 0)
len = 4 + (len / 8);
else
len = 5 + (len / 8);
multi_send_header(MULTI_TELEMETRY_RX_CHANNELS, len);
for (uint8_t i = 0; i < len; i++)
Serial_write(packet_in[i]); // pps, rssi, ch start, ch count, 16x ch data
}
#endif
#ifdef AFHDS2A_FW_TELEMETRY
void AFHDSA_short_frame()
{
multi_send_header(packet_in[29]==0xAA?MULTI_TELEMETRY_AFHDS2A:MULTI_TELEMETRY_AFHDS2A_AC, 29);
for (uint8_t i = 0; i < 29; i++) // RSSI value followed by 4*7 bytes of telemetry data
Serial_write(packet_in[i]);
}
#endif
#ifdef HITEC_FW_TELEMETRY
void HITEC_short_frame()
{
multi_send_header(MULTI_TELEMETRY_HITEC, 8);
for (uint8_t i = 0; i < 8; i++) // TX RSSI and TX LQI values followed by frame number and 5 bytes of telemetry data
Serial_write(packet_in[i]);
}
#endif
#ifdef HOTT_FW_TELEMETRY
void HOTT_short_frame()
{
multi_send_header(MULTI_TELEMETRY_HOTT, 14);
for (uint8_t i = 0; i < 14; i++) // TX RSSI and TX LQI values followed by frame number and telemetry data
Serial_write(packet_in[i]);
}
#endif
static void multi_send_frskyhub()
{
multi_send_header(MULTI_TELEMETRY_HUB, 9);
for (uint8_t i = 0; i < 9; i++)
Serial_write(frame[i]);
}
#endif //MULTI_TELEMETRY
void frskySendStuffed()
{
Serial_write(START_STOP);
for (uint8_t i = 0; i < 9; i++)
{
if ((frame[i] == START_STOP) || (frame[i] == BYTESTUFF))
{
Serial_write(BYTESTUFF);
frame[i] ^= STUFF_MASK;
}
Serial_write(frame[i]);
}
Serial_write(START_STOP);
}
bool frsky_process_telemetry(uint8_t *buffer,uint8_t len)
{
if(protocol!=PROTO_FRSKY_R9)
{
if(buffer[1] != rx_tx_addr[3] || buffer[2] != rx_tx_addr[2] || len != buffer[0] + 3 )
return false; // Bad address or length...
// RSSI and LQI are the 2 last bytes
TX_RSSI = buffer[len-2];
if(TX_RSSI >=128)
TX_RSSI -= 128;
else
TX_RSSI += 128;
}
telemetry_link|=1; // Telemetry data is available
#if defined FRSKYD_CC2500_INO
if (protocol==PROTO_FRSKYD)
{
TX_LQI = buffer[len-1]&0x7F;
//Save current buffer
for (uint8_t i=3;i<len-2;i++)
telemetry_in_buffer[i]=buffer[i]; // Buffer telemetry values to be sent
//Check incoming telemetry sequence
if(telemetry_in_buffer[6]>0 && telemetry_in_buffer[6]<=10)
{ //Telemetry length ok
if ( ( telemetry_in_buffer[7] & 0x1F ) == (telemetry_counter & 0x1F) )
{//Sequence is ok
uint8_t topBit = 0 ;
if ( telemetry_counter & 0x80 )
if ( ( telemetry_counter & 0x1F ) != RetrySequence )
topBit = 0x80 ;
telemetry_counter = ( (telemetry_counter+1)%32 ) | topBit ; // Request next telemetry frame
}
else
{//Incorrect sequence
RetrySequence = telemetry_in_buffer[7] & 0x1F ;
telemetry_counter |= 0x80 ;
telemetry_in_buffer[6]=0 ; // Discard current packet and wait for retransmit
}
}
else
telemetry_in_buffer[6]=0; // Discard packet
}
#endif
#if defined SPORT_TELEMETRY && (defined FRSKYX_CC2500_INO || defined FRSKYR9_SX1276_INO)
if (protocol==PROTO_FRSKYX||protocol==PROTO_FRSKYX2)
{
/*Telemetry frames(RF) SPORT info
15 bytes payload
SPORT frame valid 6+3 bytes
[00] PKLEN 0E 0E 0E 0E
[01] TXID1 DD DD DD DD
[02] TXID2 6D 6D 6D 6D
[03] CONST 02 02 02 02
[04] RS/RB 2C D0 2C CE //D0;CE=2*RSSI;....2C = RX battery voltage(5V from Bec)
[05] HD-SK 03 10 21 32 //TX/RX telemetry hand-shake bytes
[06] NO.BT 00 00 06 03 //No.of valid SPORT frame bytes in the frame
[07] STRM1 00 00 7E 00
[08] STRM2 00 00 1A 00
[09] STRM3 00 00 10 00
[10] STRM4 03 03 03 03
[11] STRM5 F1 F1 F1 F1
[12] STRM6 D1 D1 D0 D0
[13] CHKSUM1 --|2 CRC bytes sent by RX (calculated on RX side crc16/table)
[14] CHKSUM2 --|*/
//len=17 -> len-7=10 -> 3..12
uint16_t lcrc = FrSkyX_crc(&buffer[3], len-7 ) ;
if ( ( (lcrc >> 8) != buffer[len-4]) || ( (lcrc & 0x00FF ) != buffer[len-3]) )
return false; // Bad CRC
if(buffer[4] & 0x80)
RX_RSSI=buffer[4] & 0x7F ;
else
v_lipo1 = (buffer[4]<<1) + 1 ;
#if defined(TELEMETRY_FRSKYX_TO_FRSKYD) && defined(ENABLE_PPM)
if(mode_select != MODE_SERIAL)
return true;
#endif
}
if (protocol==PROTO_FRSKYX||protocol==PROTO_FRSKYX2||protocol==PROTO_FRSKY_R9)
{
telemetry_lost=0;
//Save outgoing telemetry sequence
FrSkyX_TX_IN_Seq=buffer[5] >> 4;
//Check incoming telemetry sequence
uint8_t packet_seq=buffer[5] & 0x03;
if ( buffer[5] & 0x08 )
{//Request init
FrSkyX_RX_Seq = 0x08 ;
FrSkyX_RX_NextFrame = 0x00 ;
FrSkyX_RX_Frames[0].valid = false ;
FrSkyX_RX_Frames[1].valid = false ;
FrSkyX_RX_Frames[2].valid = false ;
FrSkyX_RX_Frames[3].valid = false ;
}
else if ( packet_seq == (FrSkyX_RX_Seq & 0x03 ) )
{//In sequence
struct t_FrSkyX_RX_Frame *p ;
uint8_t count ;
// buffer[4] RSSI
// buffer[5] sequence control
// buffer[6] payload count
// buffer[7-12] payload
p = &FrSkyX_RX_Frames[packet_seq] ;
count = buffer[6]; // Payload length
if ( count <= 6 )
{//Store payload
p->count = count ;
for ( uint8_t i = 0 ; i < count ; i++ )
p->payload[i] = buffer[i+7] ;
}
else
p->count = 0 ; // Discard
p->valid = true ;
FrSkyX_RX_Seq = ( FrSkyX_RX_Seq + 1 ) & 0x03 ; // Move to next sequence
if ( FrSkyX_RX_ValidSeq & 0x80 )
{
FrSkyX_RX_Seq = ( FrSkyX_RX_ValidSeq + 1 ) & 3 ;
FrSkyX_RX_ValidSeq &= 0x7F ;
}
}
else
{//Not in sequence
struct t_FrSkyX_RX_Frame *q ;
uint8_t count ;
// buffer[4] RSSI
// buffer[5] sequence control
// buffer[6] payload count
// buffer[7-12] payload
if ( packet_seq == ( ( FrSkyX_RX_Seq +1 ) & 3 ) )
{//Received next sequence -> save it
q = &FrSkyX_RX_Frames[packet_seq] ;
count = buffer[6]; // Payload length
if ( count <= 6 )
{//Store payload
q->count = count ;
for ( uint8_t i = 0 ; i < count ; i++ )
q->payload[i] = buffer[i+7] ;
}
else
q->count = 0 ;
q->valid = true ;
FrSkyX_RX_ValidSeq = 0x80 | packet_seq ;
}
FrSkyX_RX_Seq = ( FrSkyX_RX_Seq & 0x03 ) | 0x04 ; // Request re-transmission of original sequence
}
}
#endif
return true;
}
void init_frskyd_link_telemetry()
{
telemetry_link=0;
telemetry_counter=0;
telemetry_lost=1;
v_lipo1=0;
v_lipo2=0;
RX_RSSI=0;
TX_RSSI=0;
RX_LQI=0;
TX_LQI=0;
#if defined HUB_TELEMETRY
FrSkyD_User_Frame_Start=FrSkyD_User_Frame_End=0;
#endif
}
void frsky_link_frame()
{
frame[0] = 0xFE; // Link frame
if (protocol==PROTO_FRSKYD)
{
frame[1] = telemetry_in_buffer[3]; // A1
frame[2] = telemetry_in_buffer[4]; // A2
frame[3] = telemetry_in_buffer[5]; // RX_RSSI
telemetry_link &= ~1 ; // Sent
telemetry_link |= 2 ; // Send hub if available
}
else
{//PROTO_HUBSAN, PROTO_AFHDS2A, PROTO_BAYANG, PROTO_NCC1701, PROTO_CABELL, PROTO_HITEC, PROTO_BUGS, PROTO_BUGSMINI, PROTO_FRSKYX, PROTO_FRSKYX2, PROTO_PROPEL, PROTO_DEVO, PROTO_RLINK, PROTO_OMP, PROTO_WFLY2, PROTO_LOLI, PROTO_MLINK
frame[1] = v_lipo1;
frame[2] = v_lipo2;
frame[3] = RX_RSSI;
telemetry_link &= ~1 ; // Sent
}
frame[4] = TX_RSSI;
frame[5] = RX_LQI;
frame[6] = TX_LQI;
frame[7] = frame[8] = 0;
#if defined MULTI_TELEMETRY
multi_send_frskyhub();
#else
frskySendStuffed();
#endif
}
#if defined HUB_TELEMETRY
void frsky_user_frame()
{
if(telemetry_in_buffer[6])
{//only send valid hub frames
frame[0] = 0xFD; // user frame
if(telemetry_in_buffer[6]>USER_MAX_BYTES)
{
frame[1]=USER_MAX_BYTES; // packet size
telemetry_in_buffer[6]-=USER_MAX_BYTES;
telemetry_link |= 2 ; // 2 packets need to be sent
}
else
{
frame[1]=telemetry_in_buffer[6]; // packet size
telemetry_link &= ~2; // only 1 packet or processing second packet
}
frame[2] = telemetry_in_buffer[7];
for(uint8_t i=0;i<USER_MAX_BYTES;i++)
frame[i+3]=telemetry_in_buffer[i+8];
if(telemetry_link & 2) // prepare the content of second packet
for(uint8_t i=8;i<USER_MAX_BYTES+8;i++)
telemetry_in_buffer[i]=telemetry_in_buffer[i+USER_MAX_BYTES];
#if defined MULTI_TELEMETRY
multi_send_frskyhub();
#else
frskySendStuffed();
#endif
}
else
telemetry_link &= ~2;
}
/*
HuB RX packets.
packet_in[6]|(counter++)|00 01 02 03 04 05 06 07 08 09
%32
01 08 5E 28 12 00 5E 5E 3A 06 00 5E
0A 09 28 12 00 5E 5E 3A 06 00 5E 5E
09 0A 3B 09 00 5E 5E 06 36 7D 5E 5E
03 0B 5E 28 11 00 5E 5E 06 06 6C 5E
0A 0C 00 5E 5E 3A 06 00 5E 5E 3B 09
07 0D 00 5E 5E 06 06 6C 5E 16 72 5E
05 0E 5E 28 11 00 5E 5E 3A 06 00 5E
0A 0F 5E 3A 06 00 5E 5E 3B 09 00 5E
05 10 5E 06 16 72 5E 5E 3A 06 00 5E
*/
static void __attribute__((unused)) frsky_write_user_frame(uint8_t ID, uint8_t low, uint8_t high)
{
telemetry_in_buffer[6] = 0x04; // number of bytes in the payload
telemetry_in_buffer[7] = 0x00; // unknown?
telemetry_in_buffer[8] = 0x5E; // start of payload
telemetry_in_buffer[9] = ID; // ID must be less than 0x40
uint8_t pos=10;
uint8_t value = low;
for(uint8_t i=0;i<2;i++)
{// Byte stuffing
if(value == 0x5D || value == 0x5E)
{// Byte stuffing
telemetry_in_buffer[pos+1] = value ^ 0x60;
telemetry_in_buffer[pos] = 0x5D;
telemetry_in_buffer[6]++; // 1 more byte in the payload
pos += 2;
}
else
telemetry_in_buffer[pos++] = value;
value = high;
}
telemetry_link |= 2; // request to send frame
}
static void __attribute__((unused)) frsky_send_user_frame(uint8_t ID, uint8_t low, uint8_t high)
{
if(telemetry_link&2)
{ // add to buffer
uint8_t test = (FrSkyD_User_Frame_End + 1) & 0x07;
if(test == FrSkyD_User_Frame_Start)
return; // buffer full...
FrSkyD_User_Frame_End = test;
FrSkyD_User_Frame[FrSkyD_User_Frame_End].ID = ID;
FrSkyD_User_Frame[FrSkyD_User_Frame_End].low = low;
FrSkyD_User_Frame[FrSkyD_User_Frame_End].high = high;
}
else // send to TX direct
frsky_write_user_frame(ID, low, high);
}
static void __attribute__((unused)) frsky_check_user_frame()
{
if((telemetry_link&2) || FrSkyD_User_Frame_Start == FrSkyD_User_Frame_End)
return; // need to wait that the last frame is sent or buffer is empty
frsky_write_user_frame(FrSkyD_User_Frame[FrSkyD_User_Frame_Start].ID, FrSkyD_User_Frame[FrSkyD_User_Frame_Start].low, FrSkyD_User_Frame[FrSkyD_User_Frame_Start].high);
FrSkyD_User_Frame_Start++;
FrSkyD_User_Frame_Start &= 0x07;
}
#endif
#if defined SPORT_TELEMETRY
/* SPORT details serial
100K 8E2 normal-multiprotocol
-every 12ms-or multiple of 12; %36
1 2 3 4 5 6 7 8 9 CRC DESCR
7E 98 10 05 F1 20 23 0F 00 A6 SWR_ID
7E 98 10 01 F1 33 00 00 00 C9 RSSI_ID
7E 98 10 04 F1 58 00 00 00 A1 BATT_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
Telemetry frames(RF) SPORT info
15 bytes payload
SPORT frame valid 6+3 bytes
[00] PKLEN 0E 0E 0E 0E
[01] TXID1 DD DD DD DD
[02] TXID2 6D 6D 6D 6D
[03] CONST 02 02 02 02
[04] RS/RB 2C D0 2C CE //D0;CE=2*RSSI;....2C = RX battery voltage(5V from Bec)
[05] HD-SK 03 10 21 32 //TX/RX telemetry hand-shake bytes
[06] NO.BT 00 00 06 03 //No.of valid SPORT frame bytes in the frame
[07] STRM1 00 00 7E 00
[08] STRM2 00 00 1A 00
[09] STRM3 00 00 10 00
[10] STRM4 03 03 03 03
[11] STRM5 F1 F1 F1 F1
[12] STRM6 D1 D1 D0 D0
[13] CHKSUM1 --|2 CRC bytes sent by RX (calculated on RX side crc16/table)
[14] CHKSUM2 --|
+2 appended bytes automatically RSSI and LQI/CRC bytes(len=0x0E+3);
0x06 0x06 0x06 0x06 0x06
0x7E 0x00 0x03 0x7E 0x00
0x1A 0x00 0xF1 0x1A 0x00
0x10 0x00 0xD7 0x10 0x00
0x03 0x7E 0x00 0x03 0x7E
0xF1 0x1A 0x00 0xF1 0x1A
0xD7 0x10 0x00 0xD7 0x10
0xE1 0x1C 0xD0 0xEE 0x33
0x34 0x0A 0xC3 0x56 0xF3
*/
#if defined MULTI_TELEMETRY
const uint8_t PROGMEM Indices[] = { 0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45,
0xC6, 0x67, 0x48, 0xE9, 0x6A, 0xCB,
0xAC, 0x0D, 0x8E, 0x2F, 0xD0, 0x71,
0xF2, 0x53, 0x34, 0x95, 0x16, 0xB7,
0x98, 0x39, 0xBA, 0x1B } ;
#endif
#ifdef MULTI_TELEMETRY
void sportSend(uint8_t *p)
{
multi_send_header(MULTI_TELEMETRY_SPORT, 9);
uint16_t crc_s = 0;
uint8_t x = p[0] ;
if ( x <= 0x1B )
x = pgm_read_byte_near( &Indices[x] ) ;
Serial_write(x) ;
for (uint8_t i = 1; i < 8; i++)
{
Serial_write(p[i]);
crc_s += p[i]; //0-1FF
crc_s += crc_s >> 8; //0-100
crc_s &= 0x00ff;
}
Serial_write(0xff - crc_s);
}
#else
void sportSend(uint8_t *p)
{
uint16_t crc_s = 0;
Serial_write(START_STOP);//+9
Serial_write(p[0]) ;
for (uint8_t i = 1; i < 9; i++)
{
if (i == 8)
p[i] = 0xff - crc_s;
if ((p[i] == START_STOP) || (p[i] == BYTESTUFF))
{
Serial_write(BYTESTUFF);//stuff again
Serial_write(STUFF_MASK ^ p[i]);
}
else
Serial_write(p[i]);
crc_s += p[i]; //0-1FF
crc_s += crc_s >> 8; //0-100
crc_s &= 0x00ff;
}
}
#endif
void sportIdle()
{
#if !defined MULTI_TELEMETRY
Serial_write(START_STOP);
#endif
}
void sportSendFrame()
{
static uint8_t sport_counter=0;
uint8_t i;
sport_counter = (sport_counter + 1) %36;
if(telemetry_lost)
{
sportIdle();
return;
}
if(sport_counter<6)
{
frame[0] = 0x98;
frame[1] = 0x10;
for (i=5;i<8;i++)
frame[i]=0;
}
switch (sport_counter)
{
case 0:
frame[2] = 0x05;
frame[3] = 0xf1;
frame[4] = 0x02; //dummy values if swr 20230f00
frame[5] = 0x23;
frame[6] = 0x0F;
break;
case 2: // RSSI
frame[2] = 0x01;
frame[3] = 0xf1;
frame[4] = RX_RSSI;
frame[5] = TX_RSSI;
frame[6] = RX_LQI;
frame[7] = TX_LQI;
break;
case 4: //BATT
frame[2] = 0x04;
frame[3] = 0xf1;
frame[4] = v_lipo1; //a1;
break;
default:
if(Sport_Data)
{
for (i=0;i<FRSKY_SPORT_PACKET_SIZE;i++)
frame[i]=pktx1[i];
Sport_Data -- ;
if ( Sport_Data )
{
uint8_t j = Sport_Data * FRSKY_SPORT_PACKET_SIZE ;
for (i=0;i<j;i++)
pktx1[i] = pktx1[i+FRSKY_SPORT_PACKET_SIZE] ;
}
break;
}
else
{
sportIdle();
return;
}
}
sportSend(frame);
}
void proces_sport_data(uint8_t data)
{
static uint8_t pass = 0, indx = 0;
switch (pass)
{
case 0:
if (data == START_STOP)
{//waiting for 0x7e
indx = 0;
pass = 1;
}
break;
case 1:
if (data == START_STOP) // Happens if missed packet
{//waiting for 0x7e
indx = 0;
pass = 1;
break;
}
if(data == BYTESTUFF) //if they are stuffed
pass=2;
else
if (indx < MAX_PKTX)
pktx[indx++] = data;
break;
case 2:
if (indx < MAX_PKTX)
pktx[indx++] = data ^ STUFF_MASK; //unstuff bytes
pass=1;
break;
} // end switch
if (indx >= FRSKY_SPORT_PACKET_SIZE)
{//8 bytes no crc
if ( Sport_Data < FX_BUFFERS )
{
uint8_t dest = Sport_Data * FRSKY_SPORT_PACKET_SIZE ;
uint8_t i ;
for ( i = 0 ; i < FRSKY_SPORT_PACKET_SIZE ; i++ )
pktx1[dest++] = pktx[i] ; // Triple buffer
Sport_Data += 1 ;//ok to send
}
// else
// {
// // Overrun
// }
pass = 0;//reset
}
}
#endif
void TelemetryUpdate()
{
// check for space in tx buffer
#ifdef BASH_SERIAL
uint8_t h ;
uint8_t t ;
h = SerialControl.head ;
t = SerialControl.tail ;
if ( h >= t )
t += TXBUFFER_SIZE - h ;
else
t -= h ;
if ( t < 64 )
{
return ;
}
#else
uint8_t h ;
uint8_t t ;
h = tx_head ;
t = tx_tail ;
if ( h >= t )
t += TXBUFFER_SIZE - h ;
else
t -= h ;
if ( t < 32 )
{
debugln("TEL_BUF_FULL %d",t);
return ;
}
/* else
if(t!=96)
debugln("TEL_BUF %d",t);
*/
#endif
#ifdef MULTI_TELEMETRY
uint32_t now = millis();
if ((IS_SEND_MULTI_STATUS_on || ((now - lastMulti) > MULTI_TIME))&& protocol != PROTO_SCANNER)
{
multi_send_status();
SEND_MULTI_STATUS_off;
lastMulti = now;
return;
}
#ifdef MULTI_SYNC
if ( inputRefreshRate && (now - lastInputSync) > INPUT_SYNC_TIME )
{
mult_send_inputsync();
lastInputSync = now;
return;
}
#endif
#endif
#if defined SPORT_TELEMETRY
if ((protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2||protocol==PROTO_FRSKY_R9) && telemetry_link
#ifdef TELEMETRY_FRSKYX_TO_FRSKYD
&& mode_select==MODE_SERIAL
#endif
)
{ // FrSkyX
for(;;)
{ //Empty buffer
struct t_FrSkyX_RX_Frame *p ;
uint8_t count ;
p = &FrSkyX_RX_Frames[FrSkyX_RX_NextFrame] ;
if ( p->valid )
{
count = p->count ;
for (uint8_t i=0; i < count ; i++)
proces_sport_data(p->payload[i]) ;
p->valid = false ; // Sent
FrSkyX_RX_NextFrame = ( FrSkyX_RX_NextFrame + 1 ) & 3 ;
}
else
break ;
}
telemetry_link=0;
sportSendFrame();
}
#endif // SPORT_TELEMETRY
#ifdef MULTI_TELEMETRY
#if defined MULTI_CONFIG_INO
if(telemetry_link && protocol == PROTO_CONFIG)
{
CONFIG_frame();
telemetry_link=0;
return;
}
#endif
#if defined MLINK_FW_TELEMETRY
if(telemetry_link && protocol == PROTO_MLINK)
{
MLINK_frame();
telemetry_link=0;
return;
}
#endif
#if defined DSM_TELEMETRY
if(telemetry_link && protocol == PROTO_DSM)
{ // DSM
DSM_frame();
telemetry_link=0;
return;
}
#endif
#if defined AFHDS2A_FW_TELEMETRY
if(telemetry_link == 2 && protocol == PROTO_AFHDS2A)
{
AFHDSA_short_frame();
telemetry_link=0;
return;
}
#endif
#if defined HITEC_FW_TELEMETRY
if(telemetry_link == 2 && protocol == PROTO_HITEC)
{
HITEC_short_frame();
telemetry_link=0;
return;
}
#endif
#if defined HOTT_FW_TELEMETRY
if(telemetry_link == 2 && protocol == PROTO_HOTT)
{
HOTT_short_frame();
telemetry_link=0;
return;
}
#endif
#if defined SCANNER_TELEMETRY
if (telemetry_link && protocol == PROTO_SCANNER)
{
spectrum_scanner_frame();
telemetry_link = 0;
return;
}
#endif
#if defined (FRSKY_RX_TELEMETRY) || defined(AFHDS2A_RX_TELEMETRY) || defined (BAYANG_RX_TELEMETRY) || defined (DSM_RX_CYRF6936_INO)
if ((telemetry_link & 1) && (protocol == PROTO_FRSKY_RX || protocol == PROTO_AFHDS2A_RX || protocol == PROTO_BAYANG_RX || protocol == PROTO_DSM_RX) )
{
receiver_channels_frame();
telemetry_link &= ~1;
return;
}
#endif
#endif //MULTI_TELEMETRY
if( telemetry_link & 1 )
{ // FrSkyD + Hubsan + AFHDS2A + Bayang + Cabell + Hitec + Bugs + BugsMini + NCC1701 + PROPEL + RLINK + OMP + MLINK + DEVO
// FrSkyX telemetry if in PPM
frsky_link_frame();
return;
}
#if defined HUB_TELEMETRY
if((telemetry_link & 2) && ( protocol == PROTO_FRSKYD || protocol == PROTO_BAYANG || protocol == PROTO_MLINK || protocol == PROTO_DEVO ) )
{ // FrSkyD + Bayang + MLINK + DEVO
frsky_user_frame();
frsky_check_user_frame();
return;
}
#endif
}
/**************************/
/**************************/
/** Serial TX routines **/
/**************************/
/**************************/
#ifndef BASH_SERIAL
// Routines for normal serial output
void Serial_write(uint8_t data)
{
uint8_t nextHead ;
nextHead = tx_head + 1 ;
if ( nextHead >= TXBUFFER_SIZE )
nextHead = 0 ;
tx_buff[nextHead]=data;
tx_head = nextHead ;
tx_resume();
}
void initTXSerial( uint8_t speed)
{
#ifdef ENABLE_PPM
if(speed==SPEED_9600)
{ // 9600
#ifdef ORANGE_TX
USARTC0.BAUDCTRLA = 207 ;
USARTC0.BAUDCTRLB = 0 ;
USARTC0.CTRLB = 0x18 ;
USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
USARTC0.CTRLC = 0x03 ;
#else
#ifdef STM32_BOARD
usart3_begin(9600,SERIAL_8N1); //USART3
USART3_BASE->CR1 &= ~ USART_CR1_RE; //disable RX leave TX enabled
#else
UBRR0H = 0x00;
UBRR0L = 0x67;
UCSR0A = 0 ; // Clear X2 bit
//Set frame format to 8 data bits, none, 1 stop bit
UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
#endif
#endif
}
else if(speed==SPEED_57600)
{ // 57600
#ifdef ORANGE_TX
/*USARTC0.BAUDCTRLA = 207 ;
USARTC0.BAUDCTRLB = 0 ;
USARTC0.CTRLB = 0x18 ;
USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
USARTC0.CTRLC = 0x03 ;*/
#else
#ifdef STM32_BOARD
usart3_begin(57600,SERIAL_8N1); //USART3
USART3_BASE->CR1 &= ~ USART_CR1_RE; //disable RX leave TX enabled
#else
UBRR0H = 0x00;
UBRR0L = 0x22;
UCSR0A = 0x02 ; // Set X2 bit
//Set frame format to 8 data bits, none, 1 stop bit
UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
#endif
#endif
}
else if(speed==SPEED_125K)
{ // 125000
#ifdef ORANGE_TX
/*USARTC0.BAUDCTRLA = 207 ;
USARTC0.BAUDCTRLB = 0 ;
USARTC0.CTRLB = 0x18 ;
USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
USARTC0.CTRLC = 0x03 ;*/
#else
#ifdef STM32_BOARD
usart3_begin(125000,SERIAL_8N1); //USART3
USART3_BASE->CR1 &= ~ USART_CR1_RE; //disable RX leave TX enabled
#else
UBRR0H = 0x00;
UBRR0L = 0x07;
UCSR0A = 0x00 ; // Clear X2 bit
//Set frame format to 8 data bits, none, 1 stop bit
UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
#endif
#endif
}
#else
(void)speed;
#endif
#ifndef ORANGE_TX
#ifndef STM32_BOARD
UCSR0B |= (1<<TXEN0);//tx enable
#endif
#endif
}
//Serial TX
#ifdef ORANGE_TX
ISR(USARTC0_DRE_vect)
#else
#ifdef STM32_BOARD
void __irq_usart3()
#else
ISR(USART_UDRE_vect)
#endif
#endif
{ // Transmit interrupt
#ifdef STM32_BOARD
if(USART3_BASE->SR & USART_SR_TXE)
{
#endif
if(tx_head!=tx_tail)
{
if(++tx_tail>=TXBUFFER_SIZE)//head
tx_tail=0;
#ifdef STM32_BOARD
USART3_BASE->DR=tx_buff[tx_tail];//clears TXE bit
#else
UDR0=tx_buff[tx_tail];
#endif
}
if (tx_tail == tx_head)
{
tx_pause(); // Check if all data is transmitted. If yes disable transmitter UDRE interrupt.
}
#ifdef STM32_BOARD
}
else if(USART3_BASE->SR & USART_SR_RXNE)
{ //Should not get here...
uint8_t dummy = USART3_BASE->DR;
dummy++;
USART3_BASE->CR1 &= ~ USART_CR1_RE; // Disable receive
}
#endif
}
#else //BASH_SERIAL
// Routines for bit-bashed serial output
// Speed is 0 for 100K and 1 for 9600
void initTXSerial( uint8_t speed)
{
TIMSK0 = 0 ; // Stop all timer 0 interrupts
#ifdef INVERT_SERIAL
SERIAL_TX_off;
#else
SERIAL_TX_on;
#endif
UCSR0B &= ~(1<<TXEN0) ;
SerialControl.speed = speed ;
if ( speed == SPEED_9600 )
{
OCR0A = 207 ; // 104uS period
TCCR0A = 3 ;
TCCR0B = 0x0A ; // Fast PMM, 2MHz
}
else // 100K
{
TCCR0A = 0 ;
TCCR0B = 2 ; // Clock/8 (0.5uS)
}
}
void Serial_write( uint8_t byte )
{
uint8_t temp ;
uint8_t temp1 ;
uint8_t byteLo ;
#ifdef INVERT_SERIAL
byte = ~byte ;
#endif
byteLo = byte ;
byteLo >>= 7 ; // Top bit
if ( SerialControl.speed == SPEED_100K )
{
#ifdef INVERT_SERIAL
byteLo |= 0x02 ; // Parity bit
#else
byteLo |= 0xFC ; // Stop bits
#endif
// calc parity
temp = byte ;
temp >>= 4 ;
temp = byte ^ temp ;
temp1 = temp ;
temp1 >>= 2 ;
temp = temp ^ temp1 ;
temp1 = temp ;
temp1 <<= 1 ;
temp ^= temp1 ;
temp &= 0x02 ;
#ifdef INVERT_SERIAL
byteLo ^= temp ;
#else
byteLo |= temp ;
#endif
}
else
{
byteLo |= 0xFE ; // Stop bit
}
byte <<= 1 ;
#ifdef INVERT_SERIAL
byte |= 1 ; // Start bit
#endif
uint8_t next = SerialControl.head + 2;
if(next>=TXBUFFER_SIZE)
next=0;
if ( next != SerialControl.tail )
{
SerialControl.data[SerialControl.head] = byte ;
SerialControl.data[SerialControl.head+1] = byteLo ;
SerialControl.head = next ;
}
if(!IS_TX_PAUSE_on)
tx_resume();
}
void resumeBashSerial()
{
cli() ;
if ( SerialControl.busy == 0 )
{
sei() ;
// Start the transmission here
#ifdef INVERT_SERIAL
GPIOR2 = 0 ;
#else
GPIOR2 = 0x01 ;
#endif
if ( SerialControl.speed == SPEED_100K )
{
GPIOR1 = 1 ;
OCR0B = TCNT0 + 40 ;
OCR0A = OCR0B + 210 ;
TIFR0 = (1<<OCF0A) | (1<<OCF0B) ;
TIMSK0 |= (1<<OCIE0B) ;
SerialControl.busy = 1 ;
}
else
{
GPIOR1 = 1 ;
TIFR0 = (1<<TOV0) ;
TIMSK0 |= (1<<TOIE0) ;
SerialControl.busy = 1 ;
}
}
else
{
sei() ;
}
}
// Assume timer0 at 0.5uS clock
ISR(TIMER0_COMPA_vect)
{
uint8_t byte ;
byte = GPIOR0 ;
if ( byte & 0x01 )
SERIAL_TX_on;
else
SERIAL_TX_off;
byte /= 2 ; // Generates shorter code than byte >>= 1
GPIOR0 = byte ;
if ( --GPIOR1 == 0 )
{
TIMSK0 &= ~(1<<OCIE0A) ;
GPIOR1 = 3 ;
}
else
OCR0A += 20 ;
}
ISR(TIMER0_COMPB_vect)
{
uint8_t byte ;
byte = GPIOR2 ;
if ( byte & 0x01 )
SERIAL_TX_on;
else
SERIAL_TX_off;
byte /= 2 ; // Generates shorter code than byte >>= 1
GPIOR2 = byte ;
if ( --GPIOR1 == 0 )
{
if ( IS_TX_PAUSE_on )
{
SerialControl.busy = 0 ;
TIMSK0 &= ~(1<<OCIE0B) ;
}
else
{
// prepare next byte and allow for 2 stop bits
volatile struct t_serial_bash *ptr = &SerialControl ;
if ( ptr->head != ptr->tail )
{
GPIOR0 = ptr->data[ptr->tail] ;
GPIOR2 = ptr->data[ptr->tail+1] ;
uint8_t nextTail = ptr->tail + 2 ;
if ( nextTail >= TXBUFFER_SIZE )
nextTail = 0 ;
ptr->tail = nextTail ;
GPIOR1 = 8 ;
OCR0A = OCR0B + 40 ;
OCR0B = OCR0A + 8 * 20 ;
TIMSK0 |= (1<<OCIE0A) ;
}
else
{
SerialControl.busy = 0 ;
TIMSK0 &= ~(1<<OCIE0B) ;
}
}
}
else
OCR0B += 20 ;
}
ISR(TIMER0_OVF_vect)
{
uint8_t byte ;
if ( GPIOR1 > 2 )
byte = GPIOR0 ;
else
byte = GPIOR2 ;
if ( byte & 0x01 )
SERIAL_TX_on;
else
SERIAL_TX_off;
byte /= 2 ; // Generates shorter code than byte >>= 1
if ( GPIOR1 > 2 )
GPIOR0 = byte ;
else
GPIOR2 = byte ;
if ( --GPIOR1 == 0 )
{ // prepare next byte
volatile struct t_serial_bash *ptr = &SerialControl ;
if ( ptr->head != ptr->tail )
{
GPIOR0 = ptr->data[ptr->tail] ;
GPIOR2 = ptr->data[ptr->tail+1] ;
uint8_t nextTail = ptr->tail + 2 ;
if ( nextTail >= TXBUFFER_SIZE )
nextTail = 0 ;
ptr->tail = nextTail ;
GPIOR1 = 10 ;
}
else
{
SerialControl.busy = 0 ;
TIMSK0 &= ~(1<<TOIE0) ;
}
}
}
#endif // BASH_SERIAL
#endif // TELEMETRY
Reference in New Issue View Git Blame Copy Permalink